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ABSTRACT 


This thesis consists of teaching tools designed to allow spacecraft engineering students 
to visualize the various phenomena associated with spacecraft dynamics. It does so via 
the use of state of the art three dimensional computer graphics on Silicon Graphics 
computers. The thesis discusses the principles in dynamics that were implemented and the 
key design considerations. A central goal was to develop applications that were user 
fnendly. A library of functions were developed called Dynamics Programming Library or 
DPL. DPL shields the users from the details of computer graphics, thus allowing them to 
concentrate on the dynamics of the problem. DPL was used to write three main 
applications; Euler, Frame, and Gyro. Euler demonstrates the representation of 
orientation using Euler angles and quaternion rotation. Gyro demonstrates the effects of 
torques applied to varying rigid body geometries and inertias. Frame allows students to 
view the motion of an object from different frames of reference. A group of 21 spacecraft 
engineering students participated in a lab exercise using these programs. Within 20 
minutes, the students could run the simulations thus validating their user friendliness. 
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L INTRODUCTION 


This thesis consists of teaching tools designed to allow spacecraft engineering students 
to visualize the various phenomena associated with spacecraft dynamics. It does so via 
the use of state of the art three dimensional computer graphics on Silicon Graphics 
computers. The system is fully interactive and incorporates the ftmdamental laws of 
dynamics. 

The system consists of four main parts. The first is a Dynamics Programming Library 
(DPL) that allows students to write programs that graphically demonstrate particle and 
rigid body dynamics. The language is designed to be used by students with a background 
in dynamics, but does not require them to have a knowlege of computer graphics. 

The other three parts of the system consist of three programs written utilizing DPL. 
The first program demonstrates the representation of orientation using euler angles and 
quaternion rotation. The second program demonstrates the effects of torques applied to 
varying rigid body geometries and inertias. This program allows the student to select a 
geometry and apply moments to the rigid body and observe the effect. The third program 
allows students to view the motion of an object fi'om different fi'ames of reference. The 
student is allowed to define the motion and view it from any fi-ame of reference. 

This thesis first discusses the principles in dynamics that were implemented. Next it 
covers the topics in computer graphics central to DPL. An important point is that users of 
DPL are shielded fi'om these details. It then goes on to discuss the key design 
considerations of the project. 
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n. DYNAMICS 


A. INTRODUCTION 

This chapter will discuss certain key principles in dynamics and kinematics 
demonstrated by this thesis. It will deal with the concept of a frame of reference as well as 
the various methods of representing an object's orientation. Finally, particle and rigid 
body dynamics will be discussed. 

B. FRAMES OF REFERENCES 

The motion of a person driving down the highway at 55 mph can be described in 
different ways. Relative to the surface of the earth she is moving at 55 mph in a given 
direction. But relative to the car she is not moving at all. Which one is correct? Can she 
at the same time be moving at 55 mph and 0 mph? The answer is that they are both 
correct. When you consider her motion in the earth's coordinate system or frame of 
reference she is traveling at 55 mph. When you consider her motion in the frame of 
reference of the car, it is indeed 0 mph. However, the car motion in the earth's frames of 
reference is 55 mph. By adding the two together we get 55 mph, the same value as in the 
first case. The description of the motion of a body is not complete without also discussing 
the frame in which the motion is described. 

There are an infinite number of frames that can be used to describe a body's motion. 
The key is to pick the frame that reveals the most about what the body is doing. A well 
chosen fi-ame of reference can also simplify the interpretation of a complicated motion. 
There are two fi^es that are very useful in both regards. The first is an inertial frame of 
reference. This is a frame in which Newton's laws of motion hold. In the example of the 
woman driving the car, the earth would be considered an inertial frame of reference. Body 
coordinates are the other important frame of reference. This is a coordinate system fixed 
to a body. The location of objects fixed to the body described in body coordinates do not 
vary. Take for example the car and the location of the driver and passenger seats. If you 
are driving north in earth coordinates the driver's seat is west of the passengers; if you are 
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driving south, h is east of the passeng«^ seat. However, in the car's body coordinate 
sy^em the location of the driver's seat does not change. If you are driving north, the 
driver's seat is left of the passenger's seat. And if you are driving south it is still left of the 
passenger's seat. 

C. OMENTATION REPRESENTATION 

When dealing with bodies that are not treated as a point mass, location of the carter of 

mass is not sufficient information to describe what is happening to the body. A means is 
needed to describe the attitude or the orientation of the body. In goieral terms this can be 
thought of as describing the orientation of the body's coordinate system relative to aixrther 
coordinate syston. There are three primary methods of representing a coordinate system's 
orientation; Direction Cosine Matrix, Euler Angles, and Quaternions. 

1. Direction Cosine Matrix 

Given two coordinate systems A and B, each defined by a set of mutually 
orthogonal unit vectors, the Direction Corine Matrix (DCM) rdates the orientation of 
one coordinate system to the other (see Figure 2.1). 



Figure 2.1 Coordinate Systems. 


One interpretation of a DCM is as a matrix that can be used to change the baris of 
a vector fi’om one coordinate system into anothor. For example, [V]a is a vector 
repressed in the A coordinate system. 

[VIa ~ ^«2*2 "*■ C»3*3 (21) 
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is tlK DCM that changes the basis of a vector from the A coordinate ^^stem to 
that of the B system, equation below transforms [V]^ into B coordinates. 

= ( 2 . 2 ) 
[V]B = C„b, + C„b, + C„b,. (2 3) 


[V]^ is DOW expressed in B coordinates. coincides with [V]^; they are just 
expressed in different coordinate systems. 

The question is how is constructed? The matrix below shows the elements in 
the DCM. Each element of the matrix is derived from the dot product between the two 
unit vectors ^own. [Ref 1: p. 9] Each dement is called a direction cosine because it is 
the dot product of two unit vectors. 


[V], = *R^^ 


C« 


b, Sj b, aj b, a. 


C., 


s= 

b, a, b, aj b, aj 


Cu 



bj A| bj bj Aj 




(24) 

(2.5) 


2. Euler Ang^c Representation 

Another interpretation of the transformation matrix is a rotation about one or 
more of the axes of a coordinate system, see Figure 2.2. Each of these rotations can be 
represented by a three by three matrix. These are called the elementary rotation matrices 
and are listed on the next page. [Ref 1: p. 10] 



Figure 2.2 Single rotation about the A coordinate axes 
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( 2 . 6 ) 



1 

0 

0 


0 

COS({>, 

-sin(]>, 


0 

sin<(>, 

COS(l>, 


COS<j)2 

0 

sin({)2 

®R^(<|),) = 

0 

1 

0 


-sin(J)j 

0 

COS(J)j 


COS(})j 

-sin(j>j 

0 


sin({)3 

COS({)j 

0 


0 

0 

1 


(27) 


( 2 . 8 ) 


An Euler angle rotation is a sequence of rotations about the axes of one 
coordinate system to eventually align them with another system. As shown in Table 2.1, 
there are 12 possible Euler rotation sequences; six non repeating and six repeating. They 
are identified by that axes about which they rotate. 


Non-Repeating 

Sequences 

Repeating 

Sequences 

123 

121 

132 

131 

213 

212 

231 

232 

312 

313 

321 

323 


Table 2.1 Euler rotation sequences 


®R^(123) represents the rotation matrix necessary to transform vectors defined in 
coordinate system a to b. ®R^(123) is constructed by multiplying three elementary 
rotation matrices. In order to transform vectors defined in a coordinate system to b 
multiply them by ®R'^(123). By taking the transpose of ®R*(123) is the matrix to transform 
vectors defined in b coordinate system to a. In other words *R®(123) = (®R^(123))\ and 
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v^ere 


[V]3 = »R^123)[V]^ 


(2.9) 


®R^123)= 


COS^3 

-sin^3 

0 

COS^2 

0 


sin^3 

COS^3 

0 

0 

1 

0 

0 

0 

1 

1 

0 

COS^3 



1 0 0 

0 cos^, -sin^i (2.10) 

0 sin^j cos^, 


A problem with Euler rotations is that there is a singularity in the angular rates and 
that the solution for every orientation is not unique. In order to clarify this point, 
meanings will be associated with the coordinate axes. Roll, Pitch and Heading will be 
used instead of 1,2, 3 (see Figure 1.3). 



Figure 1.3 Location of Roll, Pitch, and Heading Axes. 

The heading a»s comes out of the belly of the aircraft and is perpendicular to the 
horizontal direction of flight. If the rotation about the pitch axis is 90°, which means the 
aircraft is flying vertically, heading no longer has meaning because there is no horizontal 
direction of flight. A small increase or decrease in pitch from the singularity points cause 
the value in heading to change r^idly. For exan:q}le, if the aircraft had a heading of 0° 
prior to the singularity, a small decrease in pitch would return to heading to 0*^ whereas, a 
small increase would change the hea^g to 180”. There are two changes like this when a 
plane performs a loop. 
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3. Quaternion Representation 

Most of the time, the singularity wth Euler angles is not a problem, but w^en h is 
quaternions provide an alternative representation. A quaternion Q consists of two parts; 
a scalar and a vector q. Transformation between coordinate systems is accomplished by a 
single rotation about the vector q. The scalar part of the quaternion determines the 
magnitude of the rotation (see Figure 1.4). [Ref 2:, pp. 9-12] 



Figure 1.4 Quaternion Rotation about q 


The quaternion Q can be written as follows; 


where 


Q = 


qo 

qi 

92 

9 . 


qo = cos(ft'2) 

q, = (cos of angle q makes with x axis)rin(0/2) 
qj = (cos of angle q makes with y axis)sin(d/2) 
qj = (cos of angle q makes with z axis)sin(0/2) 


( 2 . 11 ) 


( 2 . 12 ) 

(2.13) 

(2.14) 

(2.15) 


and 6 is the rotation about q, 0 < 9 < t 


The quaternion can be used to generate a rotation matrix to transform vectors 
from one coordinate system to another. The rotation matrix is defined as follows; [Ref 3; 
p.ll] 
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(2.16) 


R = 


2(qo^ + qi^ -1 
2(q,q2 - qoqs) 
2(q,q3 + qoqj 


2(q,q2 + qoqj) 2(q,q3 - qoq^) 

2(qo* + q^^ - 1 2(q243 + qgq,) 

2(q2q3 - qoqi) 2(qo' + q3') - 1 


Additionally, given a rotation matrix R, a quaternion Q can be generated using the 
following set of equations; [Ref 3: p. 11] 


qo = 0.5(1+r„-r22-r33)"' 

(2.17) 

q, = 0.5(1 - r„ + rj 2 - r 33 )'^ 

(2.18) 

q^ = 0.5(1 - r„ - + r 33 )"^ 

(2.19) 

q3 = 0-5(1 + r„ + + r33)"' 

( 2 . 20 ) 


where r^^ are the elements of the rotation matrix. 

Finally, given the angular velocity of an object in body coordinates, a simple set of 

equations describe the rate of change of the quaternion. These equations are shown 
below: [Ref 2:, pp. 9-12] 


^0=-0.5(^lO)j +^2Ct>2 +^ 30 ) 3 ) 

( 2 . 21 ) 

qi=O.S(qo(Ol +^ 20 ) 3 -^ 30 ) 2 ) 

( 2 . 22 ) 

^ 1 = 0.5(q'oQ)2 + 930)1 - 910 ) 3 ) 

(2.23) 

93 = 0.5(9oO)3 + 9i0)2 + 920 ) 1 ) 

(2.24) 


where to„ oa^, 0)3 are angular velocities in body coordinates. 


D. EQUATIONS OF MOTION 
1. Particle Dynamics 

Dynamics is concerned with the relationship between motion and the forces 
affecting motion. Before discussing the effects of forces on a particle, we need to identify 
the information necessary to completely describe the motion of a particle. There are 
important questions that have to be answered to do this. First, where is the particle 
located? Figure 1.5 shows a particle and a vector R. R is the position or location of the 
particle in the A frame. The vector runs from the ori^ of A to the particle. 
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Figure 1.5 Position Vector R 

The second question that needs to be answered is how fast and in what direction is 
the particle moving. In other words, what is the change in R with respect to time or what 
is the particle's velocity? 

A^P _ ^ (2.25) 

dt 

There is one more bit of information needed. That is how is v, the velocity, 
changing with respect to time. This is the acceleration of the particle. 

(2.26) 

^ ~ dt - d, 2 

a. Newton's Lmvs of Motion 

By keeping track of position, velocity, and acceleration, we can accurately 
describe the motion of the particle. However, this is still kinematics, not dynamics, 
because there is no mention of how applied forces affect the particle’s motion. The 
foundation for dynamics was laid by Newton with his three laws of motion: [Ref 1: p. 2] 

1. A particle at rest remains at rest and a particle in motion remains in uniform, 
straight-line motion if the applied force is zero. 

2. The force on a particle equals the mass of the particle times its inertial acceleration. 
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3. For every applied force, there is an equal and opposite reaction force. 

Newton's second law is commonly written as F = ma, where F is the applied 
force, m is the particle's mass, and a is the acceleration of the particle. This is the key 
equation for relating forces to motion. F = ma holds when only a single force is applied, 
but it also holds when several forces are applied simultaneously. A more general form of 
the equation is ZF = ma, where ZF is the vector sum of the forces. Therefore, given the 
mass of a particle and all applied forces, the acceleration can be calculated as; 

^a^ = (ZF)/w (2.27) 


Now a new velocity can be calculated by integrating the acceleration over time 
and a new position by integrating the velocity. 


^v^(/) v^(0) + f 

0 

R(/) = R(0)+j ^y^dt 
0 

R(/) = R(0) + ^v^(O)/ + 11^1 


(2.28) 

(2.29) 

(2.30) 


b. Derivatives Between Different Frames 

Parameters like velocity can be defined in any fi-ame of reference, but in order 
for Newton's laws to hold, acceleration must be with respect to an inertial fi-ame of 
reference. Thus if the velocity is defined in a non-inertial fi-ame, a method is needed to 
determine the value of the acceleration in an inertial fi-ame. If a vector is defined in the B 
fi-ame, then the operator below can be used to determine the value of the derivative of that 
vector in the A fi-ame. [Ref 1; pp. 5-8] 

)=^( ) + “<d‘x( ) (2 31) 
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2. Rigid Body Dynamics 

The previous section described the motion of a particle or point mass. 
Translational motion was considered, the change in position of the point mass. However, 
objects in the real world are not point masses, they have shape and size as well as mass. 
Because of their shape, it is not sufScient to discuss only the displacement of the center of 
mass. We must also consider changes in the attitude or orientation of the object. That is, 
we must consider their rotational motion. The relationships in rotational motion are 
similar in form to their translational counterparts and are summarized in Table 2.2 below. 
The equation for integration of angular velocity given in this table is incremental because, 
finite rotations compose as quaternions, not vectors. [Ref 4: p. 267] 


Translational 

Rotational 

mass, m 

Inertia, I 

Linear momentum, P = mv 

Angular momentum, H = lo) 

Force, F 

Moment, M 

Acceleration, a 

Angular Acceleration, a 

Velocity, v 

Angular Velocity, O) 

Position, R 

Angular Position, 6 

l,F=^ = mv 

ZM=f=I(0 + io) 

V = v„ + at 

00 = (D„ + at 

R=Ro + Voi + jaF 

A0 = Go + (Hot + 


Table 2.2 Corresponding Translation and Rotational Equations 


a. Inertia 

When dealing vdth rotational motion, it is necessary to consider the distribution 
of mass rather than simply the total mass. A body's inertia provides the required 
information on the distribution of mass about the center of mass. Inertia is expressed in a 
three by three matrix. The definition for the elements of the inertia matrix is shown below, 
where body coordinates with ori^ at the center of mass are assumed for all integrations. 
[Refl:p. 102] 
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I 


(2.32) 


J -^z^ydm -jyx dm -\zx dm 

-fxydm j(x^+z^)dlm -\zydm 

-Jxzdm -Jyz dm J (jc* -^-y^yebn 

There are an infinite number of mutually orthogonal axes that can be utilized to 
determine the value of the matrix above. However, for every body there is at least one set 
of special axes called principal body axes. If this set is used, the non>diagonal elements of 
the matrix are zero. The inertia matrix in a prindpal body axis fi'ame is shown below. [Ref 
l:pp. 104-106] 


I 


\ (y^ +2^)ibn 0 0 

0 / (x^ ■¥z^)dm 0 

0 0 \{x^+^)dm 


(2.33) 


For some geometries there are simple closed form solution to the integrals 
above. The solutions for a sphere, cylinder, and block are shown on the next page. The 
short notation for the elements of the inertia matrix is shown below. 


Ix. 

^ 

K 1 = 


(2.34) 
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Figure 2.6 Rotational Inertia 







b. Euler's Equatiota of Motion 

Euler's equations of motion are a set of three coupled, first-order, nonlinear 
differential equations that describe the effects of applied moments on the angular rates of a 
rigid body. Euler's equations are listed below in principal axis coordinates. [Ref S; p. 112] 


Mx=IxxOix +0)y,t0*(/a-/jy) 

(2.35) 

II 

+ 

• 

(2.36) 

Ml —la tOxdiyilyy “ frt) 

(2.37) 


If no forces are applied to a body, the linear velocity does not change. It 
should follow that if no external torques are applied to a rigid body, the angular velocity 
would not change. Euler's equations show that this is not the case. By solving for the 
change in angular velocity or angular acceleration and setting external torques equal to 
zero, this point becomes clear. The equations show that even with no external torques it 
is possible to change an angular acceleration. There are only three cases when angular 
acceleration equals zero. The first is tri>dal, that is when to, * ca^ = w, - 0. The second is 
when there is inertia symmetry; I** ® I yy “ I The third case is when all of the angular 
velocity is about a single principal axis. Other than these three cases, there will be an 
angular acceleration even without external torques. This effect causes periodic changes in 
the three angular velocities. 


d)x — “ Jyy)IJxx 

(2.38) 

0)y = ~(0x(0x(/joe ~ iztitiyy 

(2.39) 

d)j — ■“C0x0)j((/jjF - /xt)/'fs 

(2.40) 


E. SUMMARY 

The programs in this thesis are primarily force driven. This chapter covered the key 
physical concepts that were implemented. These concepts were discussed without regard 
for computer science. It is imperative to understand these concepts independent of the 
computo’, because pl^cs and computers reade in two different worlds; one continous 

and the other discrete. There are times when there is not a one to one mapping between 
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woilds, and things that are straightforward in physics are anything but in the computer's 
worid. To bridge this gap, programmers must understand exactly what should happen, so 
that he or she can make the necessary adjustments to the computer code. 
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m. COMPUTER GRAPHICS 


A. INTRODUCTION 

This chapter discusses the system requirements for the tools developed in this thesis. 
It also describes the basic procedures required to implement a graphics simulation on a 
Silicon Graphics computer. 

B. SYSTEM REQUIREMENTS 

1. Hardware 

The programs in this thesis were developed on a Silicon Graphics Elan computer. 
Below are the specifications for the Elan that the programs were compiled and ran on. 
These are the minimum system requirements. The graphics board is critical. The 

programs will not run properly on a system that is not capable of Z Buffering. 

1 50 MHz IP20 Processor 

FPU: MBPS R4010 Floating Point Processor Chip 

CPU: MBPS R4000 Processor Chop 

Data Cache: 8 KB 

Instruction Cache: 8 KB 

Secondaiy Cache: 1 MB 

Main Memory: 64 MB 

Iris Audio Processor: revision 10 

Graphics Board: GR2-Elan 

Mouse 

2. Software 

Operating System: Silicon Graphics Irix ver 4.05 
CompUer: Silicon Graphics C++ Compiler ver 3.0 

C. 3D GRAPHICS PROCESS 
1. Screen Coordinates 

When you are sitting at a computer looking at your monitor, at times it is like you 
are peering into another world. For aO intents and purposes, h is a worid complete with 
hs own coordinate system. Figure 3.1 shows the orientation of the coordinate system 
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utilized by the Silicon Graphics (SGI) computers. Positive Y is up, positive X is to the 
right, and positive Z is coming out of the screen. 



Figure 3.1 Graphics Coordinate System 

2. Matrix Stack 

The heart of the 3D gr^hics is the stack of matrices used to construct the scene. 
The top matrix contains the information necessary to display objects on the screen. SGI's 
utilize a four by four matrix. The matrix in Figure 3.2 shows the typical matrix structure. 
The three by three area contains the data for rotations. It is very similar to the rotation 
matrices generated by an Euler rotation, but there is one significant difference. The matrix 
is the transpose of the Euler rotation matrix in Chapter n. The reason for this is simple. 
Matrix multiplication is not commutative. In Chapter 2, a rotation matrix R would 
multiply a vector V to perform the rotation. The order would be R * V. On SGI's the 
rotation matrix does not multiply, it premultiplies the matrix on the top of stack S. Its 
order is S * R instead of R * S. Thus the transpose must be used to have the same effect. 
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Figure 3.2 SGI Matrix Structure 

The first three cells of the bottom row iit Figure 3.2 show the elements of the 
matrix that contains the x, y, and z coordinates of object being drawn. The matrix above 
contains the following values for the object's location; x 1, y » 4, z » 2. The last 
column of this matrix is always (0, 0, 0, 1)^ as shown, and is needed to properiy combine 
rotation translation into one homogenous trcov^arm matrix. 

3. Double Buffering 

Moving scales are constructed fi’om a sequence of still pictures or frames. In 
order to get smooth-looking motion, the graphics display system must be able to produce 
at least 30 fi^ames per second. There are different methods for displaying these fiwnes. 
SGTs utilize a display technique called double buffering. This approach uses two buffers; 
a dntw buffer and a display buffer. The display buffer contains the data for the firame 
currently being displayed. The draw buffer is where the images for the next friune to be 
viewed are stored. When all of the images for the next fiwne are stored in the draw 
buffer, the two buffers switch roles. The draw buffer becomes the display buffer and the 
display buffer becomes the draw buffer. 

4. Viewing Process 

In order to view a 3D graphics scene on a SGI, there are certain steps that must be 
followed. This section outlines these steps. 

a. InitializiHg Graphics System 

Before ai^ images can be displayed the system must be placed in the graphics 
oKide. This process is called This process determines basic settings for 

gnqihics ^em like location and rize of the riewing window. Additionally, the SGI is 
configured for various desired modes of operation such as double bufforing. Finally, input 
devices like a mouse, keyboard, or ^acd)all are queued up so the ^em can receive data 
fi'om them. For details on the actual configuration utilized refer to ^pendix J. 
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b. Setting Penpective 

Perspective is a SGI function that determines how and which objects are to be 
displayed. Unlike initializing, which occurs once at the beginning of the graphics 
application, the perspective is set at the beginning of every frame. The function is shown 
below with the four parameters it requires. [Ref 6: pp. 7-4 - 7-6] 

perspective(Fie/rf of View, Aspect Ratio, Near-Clipping-Plane, Far-Clipping-Plane) 

The arguments of this function are defined as follows; 

(1) Field of View - This is the width of the scene being viewed in tenths of 
degrees. A typical value is 450 (45°). 

(2) Aspect Ratio - This determines the height (y direction) to width (x 
direction) ratio or the viewing screen. A value of one means that length takes up as many 
pixels in the x directions as it does in the y direction. However, 1.25 instead of one is 
typically used. This is because the pbcels on the screen are not square. Their height is 
25% longer than their width. Therefore to make objects appear to have the proper 
dimensions a 1.25 ratio is used. 

(3) Near-Clipping-Plane - This determines the location of the 
near-clipping-plane. Objects that are closer to the observer than the near-clipping-plane 
are not displayed. These objects are considered in front of the near-clipping-plane. 

Objects that are farther away from to the observer than the near-clipping-plane are 
displayed. These objects are considered behind the near-clipping-plane. 

(4) Far-Clipping-Plane - This determines the location of the 
far-clipping-plane. Objects that are closer to the observer than the far-clipping-plane are 
displayed. These objects are considered in front of the far-clipping-plane. Objects that are 
farther away from to the observer than the far-clipping-plane are not displayed. These 
objects are considered behind the far-clipping-plane. Both the near and far clipping planes 
help to avoid drawing objects that do not affect the scene being constructed. 

Before the perspective function can be executed, a unit matrix must be loaded 
on the stack. A unit matrix has ones down the diagonal and zeros everywhere else. 
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c Setting Lookat 

Once the perspective is set, the lookat fiinction sets the point of view from 
which the scene will be observed. The lookat function has the following format. [Ref 6; 
pp. 7-4 - 7-6] 

lookat(F/eH'/wi/i/, Reference Point, Twist) 

(1) View point - The view point is the x, y, and z location of the observer 

(2) Reference Point - The reference point is the x, y, and z location of the 
point being observed. 

(3) Twist - The twist is amount the scene is rotated in tenths of degrees. 
Normally it is zero, but there are times when the scene needs to be rotated. Take for 
example, an observer on an aircraft looking at the horizon. If the aircraft is flying level, no 
rotation is required. But if the aircraft banks 15° to the right, the scene has the be rotated 
an equal amount to the left to have the horizon displayed properly. 

dL Preparing Objects for Display 

The definition of each object is contained in an file. An off file is a text file 
that contains the color, location, and orientation of all of the polygons that make up the 
object. There are several functions available for the proper display of objects. The next 
two functions must be executed after initialization once for each object. The read object 
function reads the off file into memory and assigns an OBJECT pointer to it. OBJECT is 
a type defined by the SGI graphics library. A special OBJECT pointer is lightobj. It is the 
object which serves as a lighting source. A light source is required to display the other 
objects. Once the off file is read into memory, the recufy object Jor jUspIay function is 
then executed. An example of a typical execution sequence is shown below. 

OBJECT* destroyer; 

destroyer = read_object("ship.off'); 

ready_object_for_display(destroyer); 
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e. Drawing Objects 

Before objects can be drawn in the draw buffer, the perspective and lookat 
functions must be executed. At this point, the matrix on top of the matrix stack is set to 
display the scene. All objects drawn during this frame will use this matrix as a basis. 
Before proceeding, a copy of the matrix must be made. This is accomplished through the 
use of a pushmatrix function. The pushmatrix function adds a matrix to the top of the 
matrix, this matrix is identical to the former top. 

The top matrix must be modified in order to allow the object to be displayed at 
the proper location and orientation. The object is moved to the correct location with the 
translate function. The x, y, and z coordinates of the object are the three parameters the 
translate function requires. The functions premultiplies the matrix on top of the stack by 
the matrix shown below. [Ref 6: p. Cl] 



Figure 3.3 Translation matrix 

If the object is not in its original orientation, there is a rotate function available 
to place it in the correct orientation. The amount of the rotation in tenths of degrees and 
the axis of rotation are the two required arguments. Again the function premultiplies the 
top of the matrix stack. There are three possible matrices, one for each axis, they are 
{^own below. [Ref 6: p. C2] 
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Figure 3.4 Rotation matrices 



(3.1) 


(3.2) 


(3.3) 


After the translation and rotations, the object is ready to be drawn to the draw 
bufier. The displayJhisjobject function does this. It takes as its only argument the 
OBJECT pointer for the object to be drawn. This function uses the matrix on top of the 
stack to draw the object at its proper location and orientation. 

The next step is to remove the matrix from the top of the stack. The popmatrix 
function does this. Finally, the swapbuffers function is executed. It switches the draw and 

display buffers. Below is an example display sequence of commands for a house, tree, car. 

loadmatrix(unit); //unh is a unit matrix 
perspective<fov, aspect, near, far); 
lookat(v^ Vy, r^ r,, r^ twist); 
pushmatrixO; 

translate(house_x,house_y,house_z); 

di^>lay_this_object(house); 

popmatrixO; 

pushmatrixO; 

translate(tree_x,tree_y,tree_ 2 ); 

diaplay_1his_object(tree); 

popmatrixO; 

pushmatrixO; 
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traiislate(car_x,car_y,car_ 2 ); 

rotate(300,'Y"); 

di^lay_this_(^ject(car); 

popmatrixO; 

swapbuffersQ; 

All of the functions discussed in this section are part of the SGI graphics 
library. There are more functions avsdlable, in the SGI's Graphics Library Programming 
Guide . 

D. SUMMARY 

This chapter has discussed graphics fundamentals needed to construct a graphics 
simulation. An important part of this thesis is that these details are hidden from users; 
hence the simulation is user friendly. There are only a few gri^}hics calls required. This 
allows users to concentrate on the physics of the problem instead of graphics. 
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IV. IMPLEMENTATION 


A. INTRODUCTION 

Dynamics Programming Ubrcay (DPL) is the name ^ven to the €++ Classes 
implemoited by this thesis. This chapter discusses the DPL design considerations. It 
covers C-h- class design and key concepts that were implemented in DPL. Finally, it 
discusses the method used to validate the software. 

B. C-H-CLASS DESIGN 
1. vectorSD Class 

Three dimensional vectors and their operations are found throughout dynamics. 
Position, velocity, acceleration are all usually represented by a three dimensional vector. 
Since their use is common, a C++ class was developed to support vector operations. The 
class name is vector3D. The table shows the manber variables and their type. 


vectorSD 

Member Variable 

Type 

X 

double 

y 

double 

z 

double 


Table 4.1 vectorSD structure 


All of the classes in this thesis use doubles, because floats don't provide sufBcient 
accuracy. When integrating, some of the time steps are veiy small. If floats are used these 
small steps may be rounded to zero. 

The vector3D class supports many vector operations like dot product, cross 
[xroduct, and normalization. For a complete list of vector3D functions refer to Appendix 
A. 
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2. quaternion Class 

The quaternion representation for orientation was chosen over the Euler angle 
representation for two reasons. First, all orientations have a unique solution and there are 
no singularities in their rates like there are with Euler angles. Secondly, their rate changes 
are calculated algebraically instead of trigonometrically, which means they execute &ster. 
The member variables for the quaternion class are shown below. For a complete list of 
quaternion functions refer to Appendix A. 


quaternion 

Member Variable 

Type 

qO 

double 

qi 

double 

q2 

double 

q3 

double 


Table 4.2 quaternion structure 


3. inatrix3x3 Qass 

Three-by-three matrices are use often in dynamics. The matrix3x3 was developed 
to support matrix operations. Operations supported include matrix multiplication, scalar 
multiplication, and others. For a complete listing of matrix3x3 functions refer to 
Appendix A. The member variables of the class are listed in the table below. 


matrix3x3 

Member Variable 

Type 

m[9] 

double 


Table 4.3 matrix3x3 structure 


Matrices are usually accessed using two values. For example, the cell in the 
second row, third column of a matrix temp is normally accessed as follows: temp[l][2] 
(rows and columns start at the number zero instead of one). However, C++ does not 
support the double bracket convention. Therefore a single value is used instead of two. 
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To access the cell in the second row, third column of a matrix temp the matrix3x3 format 
is ten^[5]. 

4. rigid_body Qass 

To avoid confusion, "rigidjbody" (writtoi like this with an underscore) refers to 
the C-H- class and "rigid body" refers to the dynamics interpretation. The rigid_body class 
is the heart of the DPL. The class is designed to ^ulate both particle and rigid body 
dynamics. In addition to the dynamics, the elements necessary for three dimensional 
graphics support are encapsulated in this class. The table below shows the monber 
variables of the class. 


rigidjbody 

Member Variable 

Typt 

mass 

double 

location 

vectorSD* 

velocity 

vectorSD 

acceleration 

vectorSD 

force 

vectorSD 

orientation 

quaternion 

ang_velocity 

vectorSD 

ang_acceleration 

vectorSD 

moment 

vectorSD 

inertia 

matrixSxS 

size 

vectorSD 

sur&ce_area 

double 

shape 

OBJECT* 

display_axis 

int 

display_shape 

int 

typejbody 

int 

holderl 

vectorSD 

holder2 

vectorSD 

holders 

quaternion 


Table 4.4 rigidjbody class structure 
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a. nysical Attributes 

The member variables of this class &U into two main groups. The first group is 
the physical attributes. These variables are the ones required to accurately describe a rigid 
body. There are two main coordinate systems used by this class; inertial or world 
coordinates and body coordinates. The variables are defined in world or body coordinates 
depending on which one is more appropriate. 

(1) mass - This variable contains the mass of the rigid body in kilograms. 

(2) location - location is a the position of the rigid body in world coordinates. 

It is repressed in meters. Location is pointer to a vector3D instead of a vectorSD. This is 
done to allow easy tracking of rigid bodies. If a particular rigid body is the center of 
interest of a scene, a pointer for the reference point of the scene can be assigned once as 
the location pointer of the ri^d body and it will keep the rigid body in frame without 
further assignments. 

(3) velocity - velocity is defined in world coordinates and is expressed in 
meters per second. 

(4) acceleration - acceleration is defined in world coordinates and is expressed 
in meters per second squared. 

(5) force • force is defined in world coordinates and is expressed in Newtons. 

(6) orientation > this variable uses a quaternion to maintain the rigid body’s 

attitude. 

(7) ang_velocity - The angular velocity of the rigid body is defined in body 
coordinates and is in radians per second. 

(8) ang_acceleration- The angular acceleration of the rigid body is defined in 
body coordinates and is in radians per second^. 

(9) moment- The moment applied to the ri^d body is defined in body 
coordinate and is in Newton meters. 

(10) inertia - The moments of inertia of this class are expressed in principal axis 
coordinates. 

(1 l)rize - The size is defined as a vector3D to allow individual scaling of length 
in the X, X and z direction. 
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(12) suiface_area - The amount of surface is expressed in meters^. 

(13) shape • shape is the 3D graphics representation of the rigid body. 
h. Housekeqring Variables 

The housekeeping variables are variables that are required for smooth 
operation of the rigidjbody class, but do not have any real physical imerpretation. 

(1) display_axis - In order to assist in the visual determination of the rigid 
body's orientation, it is useful to see the body axes. This variable determines whether the 
body axes are displayed or not. The axes are color coordinated; the x axis is red, the y 
axis is blue, and the z axis is black. 

(2) display_shape - This variable determines whether the rigid body's shape is 
displayed. Its main use is when scenes are viewed from the point of view of a rigid body. 

If a scene is viewed from the center of a ri^d body and that rigid body is displayed, all 
that will be observed is the inside of the ri^d body. In order to view the scene properly, 
the rigid body the scene is viewed from should not be displayed. 

(3) typejbody - This variable allows the system to treat different body types 
differently. For example, the computation of the elements of the inertia matrix depends on 
the shape of the body. This variable is used to distinguished between and sphere and a 
block. 

(4) holder variables - holder 1, holder2, holder3 are »ctra slots to store data. 
The first two are vectors and the third is a quaternion. They are there if more information 
is required than the physical attributes can provide. For example if both the current and 
previous values must be maintained, holderl could be used to keq) the previous position. 
The variables are used by the updatejottachedjbody function. 

C. GRAPHICS FUNCTIONS 

When a scene is viewed, there is only one point of ^dew and one reference point. 
Because of this, two global variables were defined to manipulate the viewing of the 
scenes. The ^ is the variable that represents the view point of the scene. The target is 
the refo'ence point of the scene. Both are vector3D pointers. 
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The concept of an eye and a target was meant to isolate the user from the low level 
gnq)hics functions discussed in Chapto- m. The user just has to worry about vsdio’e the 

and target of the scene is, the other details are taken care of by the DPL. The relevant 
gothics functions are presented in Appendbc A. 

D. TIME FUNCTIONS 

Like the eye and target, time is also treated as a global variable. There are three time 
variables used: oidjime, delta, realJime_ratio. The SGI has a timing signal vdiich the 
internal clock uses for a time reference. The value of the clock is incremented by one 
count each system cycle. The value of the clock is maintiuned in system ticks, not 
seconds. The old_time variable contains the value of the system clock at the last time 
check. The delta variable contuns the elapsed time in seconds since the last time check. 
Finally, the real_time_ratio adjusts system time compared to real time. Its value is 
normally one, which means the system is real time. A real_time_ratio of three would 
mean that every second of real time corresponds to three seconds of system time (3:1 
speedup). If the realjdme^ratio is .25, for every four seconds of real time elapsed one 
second of system time will d^sed (4:1 slowdown). 

E. INTEGRATORS 

An important goal of this thesis was to devdop a system with enough flexibility to 
handle the different types of problems in dynamics. There are two main approaches to 
achieving this goal. The first is to develop a collection of very specialized functions to 
handle the different cases. This approach would produce v^ accurate functions, because 
the functions could be tailored to a particular problem, such as, spring motion. However, 
this would require the user to select from a myriad of functions to chose the correct one. 
Moreover, a problon arises ^en none of the functions match a specific case exactly. 
Namely, vdiich function should be chosen? 

The second approach is to make a few genial purpose functions to handle any 
dynamics. This is the approach used in this theris. The functions are the 

woridiorses of the rigidjbody class. They take the current state of the rigid body and the 
force iq)plied to determined the next state. The state conrists of both translational and 
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rotational variables like velocity and orientation. The update_state function determines 
the next state by integrating the equations of motions discussed in Chapter n. The are 
three different integration techniques used in this thesis. 

1. Euler Method 

The Euler method is the simplest method used. It is quickest of the three and 
provides good accuracy in most cases. The only time when it failed to provide good result 
was with spinning bodies. The Euler equations of motion (equations 2.33 - 35) w«'e 
unstable using this method. Angular velocities increased without bounds to infinity. The 
update_state integrator function uses this method. It is a real time integrator. 

2. Runga Kutta Fourth Order Method 

In order to woik with spinning bodies, a different integrator is needed. The Runga 
Kutta fourth order provides the needed stability. The update_state_rk4 uses this method. 
As long as the angular velocities don't get very high, this real time method produces good 
results. 

3. Runga Kutta Adaptive Step Method 

When more accuracy is needed, the update_state_rk45 can be used. It utilizes a 
Runga Kutta adtq)tive step method, also known as a Runga Kutta fourth/fifth order. It is 
the most stable of the three integrators, but is not real time. 

F. SOFTWARE VALIDATION 

Once the software is written, the question arises, is it correct? Validation of the 
software was accomplished at every step in the software's development. The validation 
for the vector3D, quaternion, and matrix3x3 classes was straightforward. Problems were 
worked out by hand and then simple programs were written to verify the functions. For 
exanq)le, to validate the cross product function, two arbitrary vectors were chosen and 
their cross product was calculated by hand. Next a program was written to calculate the 
cross product using the same vectors. If the program's result was correct, a second test 
trial was conducted. If it was incorrect, the function was corrected and another trial was 
performed. 
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The rigidJxKly class was not as easy to validate. Simple cases for which the result 
was well known had to be used to validate the functions. If a variety of simple cases 
work, it could be assumed that the complicated cases would work also. The first case was 
a problem in particle dynamics fi’om a physics textbook. What happens to a ball thrown 
up at 25 m/s neglecting air resistance? The answer is that it rises 32 meters in 2.6 
seconds. The system calculated and displayed the same result. A rigid body test was the 
spin up of a cylind^. The cylinder had a equal to 1600. A moment of 1600 Newton 
meters was applied for two seconds. The angular velocity increased from 0 to 2.02 rads 
per sec. The calculated answer for angular velocity was 2.00 rads per sec. Cases using 
spring motion, centripetal acceleration, and othere was tested. All of them produced the 
expected results. 

G. SUMMARY 

The issues discussed in this chapter had a rignificant effect on the development of 
DPL. In particular the decision to make a few general purpose integrators makes it a 
much better "what if?" tool. Students can use it to experiment and see what happens 
when they apply a special set of conditions to a dynamics problem. The graphics 
implementation sheilds users fi-om the low level graphics details, thus allowing them to 
focus on the dynamics of the problems. 
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V. CONCLUSIONS & RECOMMENDATIONS 


A. CONCLUSIONS 

The primary goal of this thesis was to develop tools that would enhance students 
understanding of dynamics. A central goal was to keep the applications user friendly. 
The Dynamics Programming Library was used to write three main i^)plications: wler, 
gyro, frame. The user's guides for these programs are contained in Appendix B. These 
programs demonstrate different principles of dynamics. A group of 21 spacecraft 
engineering students participated in a lab exercise using these programs. Within 20 
minutes, the students could run the simulations thus validating thdr user friendliness. 
Furthermore, they felt that seeing the dynamics reenforces what they already knew. In 
particular the "euler" program demonstrated the concept of quaternion rotations, 
something which all of them had a problem visualizing. The fact that the students believe 
that it improves their understanding is the final and most important validation of the 
system. 

B. RECOMMENDATIONS 

There are three main areas that the DPL can be used for in future research. First, the 
rigidjbody class can be used as a basis for deriving new dass of vehicles, like aircraft and 
i^eeled vehicles. These new classes can be used in systems like the Naval Postgraduate 
School's to provide vehicles that move realistically. [Ref 7; pp 1 - 18] 

Composite jigidjxxfy could be another class developed. This class would address the 
problems of working with multiple rigid bodies. Finnaly the rigidjbody class could be 
used to design graphical control system programs. Such a program could be used to 
simulate the force that a spacecraft would experence in orbit. The task of the student 
would be to develop an attitude control ^stem that keeps the vehicle in the correct 
orioitaticn. 


32 




REFERENCES 


1. Wiesd, William E., Spaceflight Dynamics, McGraw Hill Inc., 1989. 

2. Cooke, Joseph M., Zyda, Michael J., Pratt, David R., and McGhee, Rob^ B., 
NPSNET; "Flight Simulation Dynamic Modeling Using Quaternions", Presence, 

V. 1, FaU 1992. 

3. Kolve, D. I., "Describing an Attitude", paper presented at the 16th Annual 
AAS Guidnace and Control Conference, Keystone, Colorado, Feb 6- 10,1993. 

4. Halliday, David, and Resnick, Robert, Fundamentals of Physics, John Wiley & 
Sons, 1988. 

5. Agrawal, Brij, N., Design of Geosynchronous Spacecraft, Prentice-Hall, 1986 

6. McLendon, Patricia, Graphics Library Programming Guide, Silicon Graphics 
Inc., 1991. 

7. Zyda, Michael; Pratt, David; Falby, John; Barham, Paul; and Kelleher, Kristen; 
"1993 NPSNET Research Group Overview", Sep 7, 1993 


33 






APPENDK A 


Function Listing 


Vector3D Class 


Function 

- ■ i„ ,-- 1 

Description 

Return 

Value 

Example Usage 

vector3D0 

Defiuilt Initializer 

vector3D 

vector3D velocity; 

vector3D(double, double, 
double) 

Initializes a vector3D using 
three doubles 

vector3D 

" 

vector3D velocity(12.0, 13.5, 
55.9); 

vector3D(const vector3D) 

Initializes a vector3D using a 
vector3D 

vector3D 

vector3D velocity = old_velocity; 
where old_velocity is a vertor3D 

operator^const vector3D&) 

Assignment operator for class 

vector3D& 

velocity = old_velocity; 
where velocity and old_velocity 
are vector3D’s 

operatorKconst vector3D&) 

Addition operator 

vector3D 

velocity = velocity + 
old_velocity; 

where velocity and old_velocity 
are vector3D's 

operator-(const vector3D&) 

Substraction operator 

vector3D 

velocity = velocity - old_velocity; 
where velocity and old_velocity 
are vector3D's 

operator*(double) 

Scalar mulitplication 

vector3D 

velocity = velocity * 2; 
where velocity is a vector3D 

operator*(const vector3D&) 

Vector dot product 

double 

dot = velocity * old_velocity; 
where velocity and old_velocity 
are vector3D's 

operator/(double) 

Scalar division 

vector3D 

velocity = velocity / 2; 
where velocity is a vector3D 

operatorKconst vector3D&) 

Vector cross product 

vector3D 

dot = velocity ^ old_velocity; 
where velocity and old_velocity 
are vector3D's 

operator«(ostreain&, 

vector3I>&) 

C++ output 

ostreain& 

cout« velocity; 

where velocity is a vector3D 


34 






















































operator[](int) 

Allows access to individual 
components of class 

Int value range is 0 - 2. 

double 

X * velocity[0]; 

this returns the first component of 
the velocity vector and assigns 
the value to x. 

velocity[l] = 33.3; 
this assigns the second 
component of the velocity vector 
the value 33.3 

magnitudeO 

Magnitude of vector3D 

double 

speed = velocity.magnitudeO; 
this assign to the float variable 
speed a value equal to the 
magnitude of the velocity vector. 

normalizeO 

Normalized vectorSD to unit 
vector 

void 

velocity. nomudizeQ; 

this normalizes the velocity to 

one. 

normalize(double) 

Normalized vector3D to a 
vector equal to the magnitude 
of the double 

void 

velocity.normaiize(3.0); 
this normalizes the velocity to 
three. 


Quaternion Class 


Function 

Description 

Return 

Value 

Example Usage 

quatemionQ 

De&ult Initializer 

quaternion 

quaternion orientation; 

quatemion(double, double, 
double, double) 

Initializes a quaternion u^g 
four doubles. The first three 
doubles are the angles in 
radians that the axis of 
rotation makes with the x, y, 
and z axes respectively. The 
fourth double is the amount 
of rotation in radians. 

quaternion 

quaternion orient8tion(12.0,13.5, 
55.9,0.0); 

iO 

1 

t 

t 

1 

Initializes a quaternion using 
a quaternion 

quaternion 

quatonion orientation^ 
old_orientation; 

>»diere oid_orientation is a 
quaternion 


































sel(double, double, double, 
double) 

Reinitializes a quaternion 
using four doubles. The first 
three doubles are the angles in 
radians that the axis of 
rotation nudces with the x, y, 
and z axes respectively. The 
fourth double is the amount 
of rotation in radians. 

void 

orientation.set(12.0, 13.5, 55.9, 
0.0); 

operatonKconst 

quatemion&) 

Assignment operator for class 

quaternion 

& 

velocity = old_orientation; 
where velocity and 
old_ori«itation are quaternion's 

operatorKconst 

quatemton&) 

Addition operator 

quaternion 

orientation= orientation+ 
old_orientation; 
where velocity and 
old_orientation are quaternion's 

1 

1 

1 

o 

Substraction operator 

quaternion 

orientation = orientation - 
old_orientation; 
where orientation and 
old_orientation are quaternion's 

operator*(double) 

Scalar mulitplication 

quaternion 

orientation *= orientation * 2; 
where orientation is a quaternion 

opaator*(const quatemioii&) 

Quaternion multiplication 

double 

dot = orientation '* 
old_orientation; 
where orientation and 
old_orientation are quaternion's 

operator«(ostreain&, 

quaternion&) 

C++ output 

ostream& 

cout« orientation; 

where orientation is a quaternion 

operator[]Cint) 

Allows access to individual 
components of class 

Int value range is 0 - 3. 

double 

X = orientation [0]; 
this returns the first component of 
the orientation quaternion and 
assigns the value to x. 

velocity[l] = 33.3; 
this assigns the second 
component of the orientation 
quaternion the value 33.3 

magnitudeO 

Magnitude of quaternion 

double 

speed « orientation.magnitudeO; 
this assign to the float variable 
speed a value equal to the 
magnitude of the orientation 
quaternion. 
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normalinO 

Nomudized quaternion to unit 
quaternion 

void 

orioitation.nonnalizeO; 

this normalizes the velocity to 
one. 

rate_of_change(double, 
double, double) 

Using the angular velocity 
given by 3 doubles to 
detanoine the change of rate 
of the quaternion. 

quaternion 

rate = 

position.rate of change(1.0, 3.3, 
8.0); 

where rate and position are 
quaternions and 1.0, 3.3, 8.0 are 
the X, y, and z components of the 
angular velocity of position. 

update(double, double, 
double, double) 

Calculates the new value of 
the quaternion using the 
angular velocity ^ven by first 

3 doubles and the time 
interval in seconds given by 
the fourth double. 

void 

position.update(l .0,7.7,0.7,0.01); 
where position is a quaternion 
and 1.0, 7.7, 0.7 is the angular 
velocity and 0.01 is the time 
interval in seconds 

rate_of_change(vector3D) 

Using the angular velocity 
given by the vector3D to 
determine the change of rate 
of the quaternion 

quaternion 

rate = 

position.rate of change(ang rate 
); 

where rate and position are 
quaternions and ang_rate is a 
vector3D that contains the 
angular velodty. 

update(vector3D, double) 

Calculates the new value of 
the quaternion using the 
angular velocity given by 
vector3D and die time 
interval in seconds given by 
the double. 

void 

position.update(ang^rate,0.01); 
where position is a quaternion 
and ang_rate is the angular 
velocity and 0.01 is the time 
interval in seconds 


Matrix3x3 Class 


Function 

Description 

Return 

Value 

Example Usage 

matrix3x30 

Default Initializer 

matrix3x3 

matrix3x3 rotation; 

matrix3x3(double, double, 
double, double, (double, 
double, double, double, 
(double, double, double, 
double) 

Initializes a nuitrix3x3 u^g 
rune doubles 

matrix3x3 

matrix3x3 rotation(12.0, 13.5, 
55.9, 0.0,12.0,13.5, 55.9, 0.0, 
99.0); 


hnatrixB 


ix3(const inatrix3x3) Initializes a niatrix3x3 using a matrix3x3 matrix3x3 rotation- old_rotation; 

matrix3x3 \^ereold rotation is a matiix3x3 


































operator*(const in8trix3x3&) 

Assignment operator for class 

matrix3x3 

& 

rotation= old_rotation; 
where rotation and old_rotation 
are matrix3x3's 

operator+(const inatrix3x3&) 

Addition operator 

matrix3x3 

velocity = rotation + 
oldjrotation; 

where rotation and old_rotation 
are matrix3x3's 

operator-(const matrix3x3&) 

Substraction operator 

matrix3x3 

rotation = rotation - old_rotation 

9 

where rotation and old_rotation 
are matrix3x3's 

operator*(double) 

Scalar mulitplication 

matrix3x3 

rotation = rotation • 2; 
where rotation is a matrix3x3 

operator*(const inatrix3x3&) 

Matrix multiplication 

matrix3x3 

new_rot = rotation * old_rotation 

9 

where new_rot, rotation and 
old_rotation are matrix3x3's 

operator*(vector3D&) 

Matrix multiplication of a 
vector 

vector3D 

new_velocity * rotation • 
old_velocity; 
where new_velocity and 
new_velocity are vector3D's and 
rotation is matrix3x3 

operator*(double) 

Scalar multiplication 

matrix3x3 

new_rot = rotation • 12.0; 
where new_rot, rotation are 
matrix3x3's 

DCM_x_rotation(double) 

Generates a DCM for a 
rotation about the x axis 

matrix3x3 

rotation.DCM_x_rotation(2.0); 
generates DCM to a rotation of 2 
radians about x axis. 

DCM_ 3 r_rotation(double) 

Generates a DCM for a 
rotation about the x axis 

matrix3x3 

rotation.DCM_y_rotation(2.0); 
generates DCM to a rotation of 2 
radians about y axis. 

i 

rM_z_rotatioii(double) 

Generates a DCM for a 
rotation about the x axis 

matiix3x3 

rotation.DCM_z_rotation(2.0); 
generates DCM to a rotation of 2 
radians about z axis. 

DCMJbody_to_worid(quatcr 

nion) 

Generates a DCM for 
converting from body to 
world coordinates 

matrix3x3 

rotation.DCM_body_to_world(or 

ientation); 

generates DCM to rotate from 
body to worid coordinates using 
the quaternion orientation. 
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DCM_worid_toJbody(quata' 

nkm) 

Generates a DCM for 
converting from worid to 
body coordinates 

matrix3x3 

rotation.DCM_world_to_body(or 

ientation); 

generates DCM to rotate from 
world to body coordinates using 
the quaternion orientation. 


converts the matrix to its 
transpose 

void 

inertia.transposeO; 
where inertia is a matrix3x3 

generate_orientationO 

Generates a quaternion wth 
an orientation equivilant to 
the orientation of the matrix 

quaternion 

new_orientation = 
DM.generate_otientationO; 
new_orientation is a quaternion 
and DM is a matrix3x3. 

q>erator«(ostreani&, 

niatrix3x3&) 

C++ output 

ostream& 

cout« velocity; 

where velocity is a nuitrix3x3 

operator[](int) 

Allows access to individual 
components of class. Int 
value range is 0 - 8. 0 is row 

1, column 1; 1 is row 1 
column 2; etc 

double 

X = inertia[0]; 

this returns the first componoit of 
the inertia matrix3x3 and assigns 
the value to x. 

inertia[5] = 33.3; 

this assigns the fifth component 

of the inertia matrix3x3 the value 

33.3 


Time Function 


Function 

Description 

Return 

Value 

Example Usage 

setjdmeO 

Used to initialize or rraet the 
time 

void 

set_timeO; 

set_deltaO 

Determines the elapsed time 
since the last setjtimeO or 
set_deltaO command was 
issued and sets the global 
variable ddta to that value 

void 

set_deltaO; 

set_ddta(double) 

Sets delta variable to the 
value of the double 

void 

set_delta(.001); 

sets delta to .001 seconds 

readjdehaO 

Reads the value the delta 
variable in seconds 

double 

elaspedjtime = read_deltaO; 

read_tiiiMO 

Reads the value the time 
variable in seconds 

double 

current_time = read_timeO; 

read_ticksO 

Reads the value the time 
variable in ticks 

int 

ticks = read_ticksO; 

















































reset^timeO 

Resets the old_time variable 
to a value equal to the current 
time minus currrent delta. 

void 

reset_timeO; 

set_real_time_fector(double) 

Set the ratio between real 
time and the systems internal 
clock. Valid values are 
greater than 0. Values less 
than one slow down 
operations. Values greater 
than one speed up operations 

void 

set_real_time_factor( 10); 

This command would make on 
second of cpu time equal to ten 
seconds of real time. 

set_real_time_£actor(0.5); 

This command would make on 
second of cpu time equal to a half 
a second of real time. 


>hics Functions 


Function 

Description 

Return 

Value 

Example Usage 

inhializeO 

Initializes the graphics 
system. 

void 

initializeO; 

init_control_windowO 

Initializes the control 
window. 

void 

init_control_windowO; 

main_windowO 

Returns control to main 
window. 

void 

main_windowO; 

controljwindowQ 

Returns control to control 
window. 

void 

control_windowQ; 

clear_control_windowO 

Clears the control window. 

void 

clear_control_windowO; 

culer_control_window(int, 
int, int, int, int, int, int, 
quaternion, double) 

Display controls for wler 
program. The first three ints 
are the three angles of 
rotations. The second three 
are the axes of rotations. The 
seventh int is the switch for 
the "show quaternion" 
feature. The quaternion is the 
orientation of the shuttle. 

The double is the value for 
theta used by the quaternion 
rotation. 

void 

euler_controls(angl, ang2, ang3, 
axisl, axis2, axis3, q, orientation, 
theta); 












































igyro_cofitrol_windowQiit, int, 
int, int, vector3D, int, int, int, 
d<xible, double, double, 
dmible) 

Di^lay controls for gyro 
program. The first three ints 
are the assigned angular 
velocities. Ilie fourth int 
determines the shape. The 
vector3D determines the size. 
The next three ints are the 
applied moments. The first 
double is the duration of 
applied moment. The second 
is the magiutude of the 
moment. The third is the time 
while the moment is being 
applied. The fourth is the 
time of the total session 

void 

gyro_controls(x, y, z, object, 
size, ml, m2, m3, duration, mag, 
elaps^, total); 

1 

stat_controls(double, double, 
double, double, double, 
double, double, double, 
vectorSD*) 

Displays the statistics for the 
active rigid_body in the gyro 
program. The fiirst three 
doubles are the body's angular 
velocities. The next is the 
angular momentum. Next are 
the moments of inertia. 

FinaUy the vector3D* is a 300 
cycle history of the angular 
velocity. 

void 

stat_controls(x, y, z, am, mass, 

Ix, ly, Iz, old_av); 

franie_control_window(int, 
int, vector3D, vector3D, 
vector3D, int) 

Display controls for fiwne 
program.The parameter in 
order are viewing level, 
asngnment level, linear 
velocity, position, angular 
velocity, viewing axis. 

void 

fiame_control_window(vlevel, 
flevel, mag, pos, av, vaxis); 

viewO 

Execute the commands for 
vewing a object to include 
lookat and swapbufifoa 

void 

viewQ; 



















view(qu8ternion, vector3D, 
bit) 

Command for viewmg a scene 
from the pobit of view of a 
rigidjbody. The quaternion is 
the orioitation of the 
ri^djbody. The vector3D is 
the location of the 
rigidjbody. The bit is the 
axis down which the scene is 
viewed. The bit values are 1, 
2,3 for the positive x, y, and 
zaxes. For views down the 
negative axes use -1, -2, or -3 

void 

view(orientation, position, 1); 

This allows the scene to be 
viewed from the point of view of 
a body at a location of position, 
looking down the positive x axis. 

set_eye(double, double, 
double) 

Sets the eye point 

void 

set_eye(0.0,0.0,0.0); 

set_target(double, double, 
double) 

Sets the target pobit 

void 

set_target(0.0,0.0,0.0); 

attach_eye_to(vector3D*) 

Change the address of the 
global eye vector3D 

void 

attach_eye_to(near) 
where near is vector3D* 

attach_target_to(vector3D*) 

Change the address of the 
global target vector3D 

void 

attach_target_to(far) 
where far is a vector3D* 

roUte_vtew(int) 

Rotates the view bi tenths of 
degrees. This function is 
used bi conjunction with the 
viewQ function. 

void 

rotate_view(450); 
rotates the view 45 degrees. 

gravity_statusO 

Return 0 is gravity is off and 

1 ifhison. 

bit 

if(gravity_statusO) 

X++; 

setjgiravity_onO 

Sets gravity variable to 1 

void 

setjgravity_onO; 

setjgravity_offO 

Sets gravity variable to 0 

void 

setjgravity_offO; 

togglejgravityO 

Changes gravity variable to 1 
if h is 0 or changes it to 0 if it 
isl. 

void 

toggle_gravityO; 

ab’_re8istance_status0 

Return 0 is afr resistance is 
off and 1 if it is on. 

bit 

if(air_resistance_statusO) 

X++; 

setjdrjrcsistance_onO 

Sets afr reristance variable to 

1 

void 


setjur_resistance_ofiO 

Sets air resistance variable to 

0 

void 

set_air_resistance_oflR3i 

tog^e_air_resistanceO 

Changes abr reristance 
variable to 1 if it is 0 or 
dianges it to 0 if it is 1. 

void 

toggle_air_resistanceO; 
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Menu Functions 


Function 

Description 

Return 

Value 

Example Usage 

initialize_menu0 

Initializes the menu system 

void 

initialize_menuO; 

queue_testO 

Checks the queue to see if a 
quwed device was signalling 
1 ^ returns the code for the 
device. Code not yet 
implemented. 

int 

selection = queuc_testO; 


R igid_Bo(^ Class Functions 


Function 

Description 

Return 

Value 

Example Usage 

rigidjKxfyO 

Creates a default rigid_body 

rigid_body 

rigidjbody ball; 

rigidj)ody0nt) 

Creates a rigidjbody of one 
of the following types: 1 - 
cube, 2 - sphere, 3 - cj^ender. 

ripdjbody 

rigidjbody ball(2); 

rigid_body(char*) 

Creates a default ri 9 d_body 
but uses a off file for the 
shape. 

ri^d body 
0 

rigidjbody baUC'beachJball.off"); 

compute_inertiaO 

Computes the inertia of the 
rigid body in principle axis's 
and assign it to the rigid body 
object. 

void 

sphere.compute_inertiaO; 
where sphere is a rigid_body 

assign_mass(double) 

Assign a mass to the 
ripdjbody 

void 

ball.assign_mass(10.0); 
assigns the ball a mass of 10 kg 

astign_size(double) 

Assign a size to the 
rigidjbody ^ere x, y, and z 
components are the same 

void 

block.assign_size(5.0); 
assign the block x, y, and z equal 
to 5 meters 

assign_size(double, double, 
double) 

Assign a size to the 
rigidjbody 

void 

block.assign_size(1.0, 2.0, 3.0); 

assign_size(vector3D) 

Assign a size to the 
rigidjbody 

void 

a5sign_tize(size) 
where size is a vector3D 

assign_surfiice_area(double) 

Assign a surfiu« area to the 
rigidjbody 

void 

ball.astign_sur&ce_area(3.5) 

assign_inertia(double, double, 
double) 

Astign a inertia to the 
rigid_body in principle axis's 

void 

block.assign inertia(1.0,2.0, 

3.0); 
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assigii_orientation(double, 
double, double) 

Assign orientation to the 
rigid_body. These are the 
values for the orientation 
quaternion 

void 

block.assign orientation(1.0, 2.0, 
3.0, 8.0); 

assign orientation(quaternion 
) 

Assign orientation to the 
ri^djbody using a quaternion 

void 

block.assign_orientation(attitude) 

9 

where attitude is a quaternion 

assign_shape(OBJECT'*') 

Assign a new shape to the 
rigidjbody 

void 

ball.assign_shape(box); 
where box is a OBJECT* 

1 v sX s 


assignJocation(double, 
double, double) 

Assign location in world 
coordinates 

void 

plane.assignJocation(0.0,0.0, 
10.0) 

where plane is a ri 9 d_body 

assign_velocity(double, 
double, double) 

Assign velocity in world 
coordinates 

void 

plane.assign velocity(0.0, 0.0, 
10.0) 

where plane is a rigid_body 

assign_acceleration(double, 
double, double) 

Assign acceleration in worid 
coordinates 

void 

plane.assign acceleration(0.0, 

0.0,10.0) 

where plane is a rigid_body 

asstgn_ang_velocity(double, 
double, double) 

Assign angular velocity in 
world coordinates 

void 

plane.assign ang_velocity(0.0, 

0.0,10.0) 

where plane is a rigid_body 

asagn_ang_acceleration(doub 
le, double, double) 

Assign angular acceleration in 
world coordinates 

void 

plane.assign ang acceleration(0. 

0,0.0,10.0) 

where plane is a rigidjbody 

assign_force(double, double, 
double) 

Assign force in world 
coordinates 

void 

plane.assignJforce(0.0,0.0,10.0) 
where plane is a rigid_body 

assign_monient(double, 
double, double) 

Assign moment in world 
coordinates 

void 

plane.assign moment(0.0,0.0, 
10.0) 

where plane is a rigid body 


assignJocation(vector3D) 

Assign location in world 
coordinates 

void 

plane.assignJocation(new_value) 
where plane is a rigidjbody and 
new_value is a vector3D 

asstgn_vdocity(vector3D) 

Assign velocity in world 
coordinates 

void 

plane.assign_velocity(new_value) 
where plane is a rigid_body and 
new_value is a vector3D 


44 















































a8sign_accderation(vector3D Assign acceleration in worid void 
) coordinates 


plane.assign_acceleration(new_va 

lue) 

where plane is a rigid_body and 
new value is a vector3D 


astign apg_yelocity(vector3 
D) 

Assign angular velocity in 
world coordinates 

void 

plane. assign_ang_velocity(new_v 
alue) 

v^^ere plane is a rigjd_body and 
new_value is a vector3D 

a8agn_ang_acceleration(vect 

or3D) 

Assign angular acceleration in 
world coordinates 

void 

plane.assign_ang_acceleration(ne 

w_value) 

where plane is a rigid_body and 
new_value is a vector3D 

assign_force(vector3D) 

Assign force in world 
coordinates 

void 

plane. assign_force(new_value) 
where plane is a rigidjbody and 
new_value is a vector3D 

assign_nioment(vector3D) 

Assign moment in worid 
coordinates 

void 

plane.assign_moment(new_value) 
where plane is a rigid_body and 
new value is a vector3D 


assign_velocityJbc(double, 
double, double) 

Assign velocity in body 
coordinates 

void 

plane.assign velocity bc(0.0, 0.0, 
10.0) 

where plane is a ri^d Jbody 

assign_accelerationJbc(doubl 
e, double, double) 

Assign acceleration in body 
coordinates 

void 

plane.assign acceleration bc(0.0, 
0.0,10.0) 

vdiere plane is a rigidjbody 

assign_ang_velocity_bc(doubl 
e, double, double) 

Assign angular velocity in 
body coordinates 

void 

plane.assign ang^velocity bc(0.0 
, 0.0, 10.0) 

^ere plane is a rigidjbody 

assignjuig^acceleration_bc(d 
ouble, double, double) 

Assign angular acceleration in 
body coordinates 

void 

plane.assign ang^acceleration be 

(0.0, 0.0, 10.0) 

where plane is a rigidjbody 

assignjrorceJbc(double, 
double, double) 

Assign force in body 
coordinates 

void 

plane.assign force bc(0.0,0.0, 
10.0) 

^ere plane is a ri^djbody 

asmgn_niomentJbc(double, 
double, double) 

Assign moment in body 
coordinates 

void 

plane.assign moment bc(0.0, 

0.0,10.0) 

where plane is a rigidjbody 








































asfflgn_velocity_bc(vector3D) 

Assign velocity in body 
coordinates 

void 

plane. assign_velocity_bc(new_val 
ue) 

where plane is a rigid_body and 
new_value is a vector3D 

assign accderation bc(vector 
3D) 

Assign acceleration in body 
coordinates 

void 

plane.assign_acceleration_bc(new 

_value) 

vdio'e plane is a rigidjbody and 
neWjValue is a vector3D 

asagn ang^vdocity bc(vecto 
r3D) 

Asdgn angular velocity in 
body coordinates 

void 

plane.asagn_ang_velocity_bc(ne 

WjValue) 

where plane is a rigidjbody and 
neWjValue is a vector3D 

assign_ang_acceIeration_bc(v 

ector3D) 

Assign angular acceleration in 
body coordinates 

void 

plane. assign_ang_acceleration_bc 
(neWjValue) 

where plane is a rigidjbody and 
neWjValue is a vector3D 

assign_forceJbc(vcctor3D) 

Assign force in body 
coordinates 

void 

plane, assign force bc(new value 
) 

v^ere plane is a rigidjbody and 
neWjValue is a vector3D 

assign_momentJ)c(vector3D) 

Assign moment in body 
coordinates 

void 

plane.assign_momentjbc(new_va 

iue) 

where plane is a rigid_body and 
neWjValue is a vector3D 



retiini_niassO 

Returns the mass of the 
rigidjbody 

double 

weight = ball.retum massO * 

9.81; 

where baU is a rigid_body and 
weight is a double 

retumJocationO 

Returns location in wortd 
coordinates 

vector3D 

postion = ball.retum_locationO; 
where ball is a rigidjbody and 
position is a vector3D 

retumJocationjJtrO 

Returns location pointer 

vector3D* 

position - 

ball.retumJocation_ptrO; 

where ball is a rigidjbody and 
portion is a vector3D* 

retum_velocityO 

Returns velocity in worid 
coordinates 

vector3D 

missle.assign_velocity(plane.retur 

n_velocityO); 

this assign the missile a velocity 
equal to that of the plane. Where 
plane and missle are rigidjbody's 
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retiim_accderationO 

Returns acceleration in world 
coordinates 

vector3D 

acc = ball.retumaccelcrationO; 
where ball is a rigid_body and acc 
is a vector3D 

rctum_ang_velocityO 

Returns angular velocity in 
worid coordinates 

vectorSD 

av = ball.retum_ang_velocityO; 
where ball is a rigid_body and av 
is a vector3D 

retum_ang^accelerationO 

Returns angular acceleration 
in world coordinates 

vectorSD 

aa = 

ball.retum_ang_accelerationO; 
where ball is a ri^djbody and aa 
is a vector3D 

retum_forccO 

Returns force in world 
coordinates 

vectorSD 

force = ball.retum_forceO; 
where ball is a rigid body and 
force is a v«:tor3D 

retum_momenv 

Returns moment in world 
coordinates 

vector3D 

torque = ball.retummomentO; 
where ball is a rigid_body and 
torque is a vector3D 

retum_orientationO 

Return orientation 

quaternion 

attitude = 

ball.retum_orientationO; 
where ball is a rigid_body and 
attitude is a quaternion 

retum_surface_areaO 

Returns surface area 

double 

sa = baU.retum_surface_areaO; 
where ball is a rigid_body and sa 
is a double 

retum_shapeO 

Returns OBJECT* of 
rigidjbody 

OBJECT* 

new_shape = ball.retum_shapeO; 
where ball is a rigid_body and 
new_shape is a OBJECT* 
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retum_velocity_bcO 

Returns velocity in body 
coordinates 

vector3D 

missle. assign_velocity(plane. retur 

n_velocity_bcO); 

this assign the missile a velocity 

equal to that of the plane. Where 

plane and misde are ri^djbodys 

retum_accelerationJbcO 

Returns acceleration in body 
coordinates 

vector3D 

acc = 

baU.retum_acceleration_bcO; 
where ball is a rigidjbody and acc 
is a vector3D 

retum_an8^vdlocity_bc0 

Returns angular velocity in 
body coordinates 

vectorSD 

av = 

ball.retum_ang;_velocity_bcO; 
where ball is a rigid_body and av 
is a vector3D 
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retum_forcc_bcO 


retum_moinent_bcO 


di^layO 


update_stateO 


update_state_rk40 


update_statejrk450 



add_axisO 


remove_axisO 


Returns angular acceleration 
in body coordinates 

vectorSD 

aa = 

ball.retum_ang_acceleration_bcO 

f 

where ball is a rigid_body and aa 
is a vectorSD 

Returns force in body 
coordinates 

vectorSD 

force = ball.retum_force_bcO; 
where ball is a rigid_body and 
force is a vectorSD 

Returns moment in body 
coordinates 

vectorSD 

torque = 

ball.retum_moment_bcO; 
where ball is a rigid_body and 
torque is a vectorSD 




displays the rigidjbody 

void 

ball.displayO; 

where ball is a rigidjbody 

Update the state varibles of 
the rigid_body using Euler 
integration 

void 

ball.update_stateO; 
where ball is a rigidjbody. 

Update the state varibles of 
the rigidjbody using Runga 
Kutta Fourth Order. 

void 

ball.update_state_rk40; 
where ball is a rigidjbody. 

Update the state varibles of 
the rigidjbody using Runga 
Kutta Fourth/Fiflh Order 
(Adaptive Step). It returns 
the step size taken in seconds. 

double 

step = ball.update_state_rk450; 
where ball is a rigid_body and 
step is a dr uble. 

Update the state varibles of 
the rigid_body using Euler 
integration. This is used 
when the motion of the 
calling rigidjbody is defined 
in the fiame of reference of 
other rigidjbody 

void 

book.update_state(car); 
where book and car are of the 
type rigid_body. 

This reinitializes all state 
variables to their starting 
conditions 

void 

shuttle.zeroO; 

where shuttle is a rigid_body 

Shows the coordinate axis's 
of the rigidjbody 

void 

ball.add_axisO; 
where ball is a rigid_body 

Removes the coordinate axis's 
of the rigidjbody 

void 

ball.remove_axisO; 

\^iiere ball is a rigid_body 
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attach_eyeO 

Attaches the global eye point 
to the rigid_body. 

void 

ball.attach_eyeO; 
where ball is a rigid_body 

attach_targetO 

Attaches the global target 
point to the rigid_body 

void 

bail.attach_targetO; 
where ball is a rigid_body 

set_eye(double, double, 
double) 

Sets the eye point to the 
given location 

void 

set_eye(0.0,0.0,0.0); 

set_target(double, double, 
double) 

Sets the target point to the 
given location 

void 

set_target(99.0,8.4, 98.0); 
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APPENDIX B 


Dynamics Visualizer User Guide 

A. EULER 

1. Description 

This program demonstrates Euler and quaternion rotations. Users can select any 
Euler sequence desired and the space shuttle wQl execute the rotation. The angle of each 
rotation in the sequence is restricted to 5” increments. The program has an option that 
demonstrates equivalent quaternion rotations. Users select an Euler sequmce and a 
shuttle executes an Euler rotation. For the "equivaloit" option, a second shuttle executes 
a quaternion rotation to achieve the same orientation. 


Angle Selector 



Figure B.l E^er Control V^dow 


2. Operation 

The program is executed by typing euler at the prompt. The user selects the three 
angles for the rotation using the up and down arrows on the angle selectors. Next the 
axes of rotation are selected. The user can change the axis of rotation by clicking in any 
of the axis selector boxes. The axes are color coded. Axis 1 is red, axis 2 is blue, axis 3 is 
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black. The color of the angle and axis selector match the axis of rotation. The colors in 
Figure B. 1 represent a 123 rotation. When the desired sequence has been selected, click 
on the "GO" button to execute the rotation. Pressing the "Reset" button returns the 
shuttle to its original orientation. 

In order to see the quaternion equivalent rotation, the user selects the "show 
quaternion equivalent box". Selecting this option causes two shuttles to appear. The one 
on the left performs Euler rotations and the one on the right performs quaternion 
rotations. The controls operate the same as before. However, this time when the user 
executes a sequence, one shuttle performs an Euler rotation and the other performs a 
quaternion rotation. 

3. Suggested Exercise 

1. Set the three angles to 10, 30,60 in order. Execute the following sequences: 123, 
321,121, 313. Notice that each sequence has a different final orientation. 

2. Select the "Show Quaternion Equivalent" box and repeat the four rotations. 

3. After the rotations are complete, experiment with different sequences and angles. 

B. GYRO 

1. Description 

This program demonstrates basic rigid body dynamics. It allows users to select 
fi-om three different geometries; a block, sphere, and cylinder. Users can change the size 
of the rigid body in the x, y, and z directions independently, thus ^ving them the ability to 
alter the distribution of mass. The mass itself is set at 1000 kilograms. Once the rigid 
body is configured, angular velocities can be assigned and moments can be applied. A 
gr^h on the bottom of the screen shows the angular velocities of the rigid body in body 
coordinates. The chart is color coded. X axis is red, Y axis is blue, Z axis is black. 
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Figure B.2 Gyro Control Window 


2. Operation 

The program is executed by typing gyro at the pron^. Users select the type of 
ri^d body by clicking on one the three choices under the shape fidd. Size, Ang Vd, 
Moment, Duration, Magnitude fields use the up and down arrows to change their values. 
Additionally, Size, Ang Vel, Moment are controlled in the x, y, and z direction 
indq)endentty. Size is the length of the ri^d body in meters along its axes. Ang Vel is the 
assigned angular vdocity in radians per sec. To begin the rigid body spinning at this rate, 
users must press the "Spin Up" button. The Moment field allows users to specify moment 
in Newton meters. Duration is the length of time that the moment is to be applied in 
seconds. Magnitude is a scaling fiictor for the Moment fidd. To apply the moment, click 
on either the "Body Moment" or the "Inertial Moment" button, depending on whether the 
moment is to be applied in body or inertial coordinates. 

Gyro is not a real-time system. At times the system slows down significantly. The 
two clocks display system time. The session clock shows the time since the program was 
executed. The elapsed clock shows the amount of time the most recent moment was 
{q}plied. Finally, the "Reset" button resets the rigid body back to its initial condition. 

3. Suggested Exercise 

1. Select the block set. Setsizetox = 2, y=.5, z = 2. Set angular velocity to y = 3 
and press spin up. Notice that the blue line appears on the graph on the bottom of 
the scene. 

2. Set angular velodty to y = 3, z = 1 and press spin up. Now there are velodties 
about all three axes. The y angular velocity is constant due to symmetry. The x and 
z angular velocities change in a sinusoidal manner. 
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3. Press reset, set moment to z = 2, duration to 1 and magnitude to 100 (le+02), thoi 
press inertial moment. The block b^jns to rotate counter clockwise. 

4. Press reset, set angular vdodty to y == 2, and press spin up. Set moment to z = 2 
and press inertial moment. This time motion amilar to that in step two is d)served. 

5. Press reset, set angular velocity to y = 8, and press spin up. Set moment to z = 2 
and press inertial moment. This time the moment have much less efifect on the 
motion of the block. 

6. Press reset. Change the ^ to x = .5, y = 2, z = .5 and rq)eat steps two through 
five. The moments will have a more profound effect on the motion. 

C. FRAME 

1. Description 

The Frame program shows motion fi^om dififerent frames of reference. The are six 
different fiames: the inertial and one to five. Users are able to define the motion of up of 
levels one through five. Each frame level is defined with r^pect to the previous fiame 
levd. The motion of level one is defined with respect to an inertial frame. The motion of 
levd two is defined with respect to level one, and so on. Once the motion is defined it can 
be viewed firom any frame level. 


Inertial Viewing Level 


i Viewing Frame 
t Level Level 

III 


Linear 

Velocity 


Angular 

Velocity Poaltlon 

s SHE [iPBI3 

[▼I 


Non Inertial Viewing Level 


Linear Angular 

Velocity Velocity Position 

Bi ■■■ 


i 


h 


A 



Co 


Reset 


viewing 

Axis 

i 


Go 


Reset 


Figure B.3 Frame Control Windows 
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2. Operation 

The program is executed by typing frame at the prompt. The scene is initially 
viewed from the inertial level. The viewing level is selects the level from which the scene 
is viewed. The frame level is the level at wWch the motion is being defined. A level one 
through five, three parameters can be specified; linear velocity, position, angular velocity 
The top control window in Figure B.3 is the one seen when the viewing level is set to 
inertial. When any other viewing level is selected, the control window changes to the one 
on the bottom of the figure. The viewing axis is an additional control that allows the user 
to select the axis down which the scene is viewed. Clicking on the "Go” button starts the 
scene moving. The "Reset" button returns it to its ori^nal configuration. 

3. Suggested Exercise 

1. Set the follow settings: frame level 1 - angular velocity to y = 1; frame level 2 - 
position to X = 2, angular velocity to z = .5, frame level 3 - position to y = .5, 
angular velocity to y = 2. Press go and observe the motion. 

2. Observe the motion from the following setting: viewing level -1, viewing axis« +X, 
viewing level - 2, viewing axis - -X; viewing level - 2, viewing axis - +Y; viewing 
level - 3, viewing axis - -Y. Observe the motion for at least twenty seconds at each 
setting. 
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APPENDIX C 


Graphic Visualizer Code 

A. EULER & QUATERNION ROTATIONS 

#include "base.H" 

^include <stdlib.h> 

voidmainO 

{ 

int section * 0, bypass = 0, NO_GO = 0. go_next = 0; 
vector3D reuse, reuse2, q, lamda, av; 
niatrix3x3 rotation; 
quaternion reuse_q; 

double duration = 0.0, xrot = 0.0, yrot = 0.0, zrot = 0.0, rot = 0.0, theta = 0, theta_dot; 
int mx = 0, my “ 0, GO = 0, show_q = 0; 

int vaxis(6], axisl = 0, axis2 = 1, axis3 = 2, angl = 0, ang2 = 0, ang3 = 0, show_axis = 0; 

initializeO; 

initialize_menu0; 

init_control_window0; 

main_window0; 

tigidjxxfy shuttle(S0), shuttle2(50), cylinder(3); 

rigidjbody firanie(100); 

shuttle.a<M_axisO; 

shuttle2.assign_location(3,0,0); 
set_cye(0.0,0.6] 8); 
set_target(0.0,0.0,0.0); 
set_timeO; 
while (section !* 99) 

{ 

section = queuejestQ; //return value if keyboard or mouse has input 

set_delta0; 

viewO; 

euler_controls(angl, ang2, ang3, axisl + 1, axis2 + 1, axis3 + 1, 
show_q,shuttle.retum_orientationO, theta); 

//when using the mouse, the system response is too fast bypass allows for a 4 cycle delay before 
/Abe next mouse input is processed. 
if(bypass > 0 lypass < 4) 
lypass++; 
else 

bypass = 0; 
ifCsection > 99999) 

{ 

mx » section /100000; //decode mouse x coordinate 
my s section - (mx * 100000); //decocte mouse y coordinate 
if(!bypass) 

{ 

bypass 1; 

i£[ny > 923 && my < 939) 
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// adjust n^on angle 
if(mx > 194 &A mx < 208) 
angl = (angl + 5) % 360; 
if(nix > 327 &A tnx < 342) 
ang2 =* (ang2 + 5) % 360; 
if(nix > 460 && mx < 476) 
ang3 = (ang3 + 5) % 360; 
if(mx > 274 && mx < 289) 
angl = (angl - 5) % 360; 
if[mx > 407 && mx < 421) 
ang2 = (ang2 - 5) % 360; 
if(mx > 540 && mx < 554) 
ang3 = (ang3 - 5) % 360; 

> 

//select axis for rotation 
il(my > 903 && my < 919) 

{ 

if(mx > 234 && mx < 249) 
axisl = (axisl + 1) % 3; 
if(mx > 367 &St mx < 382) 
axis2 = (axis2 + 1) % 3; 
if(mx > 500 &St mx < 516) 
axis3 = (axis3 + 1) % 3; 

} 

// GO button selected 

if(mx > 740 && mx < 807 && my > 890 && my < 959 && !NO GO) 

{ 

GO=l; 

NO GO = 1; 

} 

// Show Quaternion Button 

ifl[mx > 261 && mx < 276 && my > 878 && my < 892) 

{ 

show_q = (show_q + 1) % 2; 
if(show_q) 

{ 

sbuttle.zero0; 

shuttle2.zero0; 

shuttle.add_axis0; 

shuttle.assign_location(-3,0,0); 

shuttle2.assign_location(3,0,0); 

set_cye(0.0,0.oi 11); 

set timeO; 

} 

else 

{ 

shuttle.zero0; 
shuttle2.zero0; 
diuttle.add_axis0; 
set eyc(0.0,0.0,8); 

} 

angl =0; 
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aiig2“0; 
ang3 = 0; 
axisl ■ 0; 
axis2* 1; 
axis3 -2; 

NO_GO = 0; 
dui^on » 0.0; 
show axis^O; 

GO="0; 
theta «0; 

} 

// RESET button selected 

if^mx > S40 &A mx < 907 8l& my > 890 St& my < 959) 

{ 

if|[show q) 

{ 

shutOe.zeroO; 

shuttle2.zero0; 

shuttle.add_axisO; 

shuttle.assign_location(-3A0); 

shuttle2.assign_location(3,0,0); 

set_eyc(0.0,0.0.11); 

set timeO; 

} 

else 

{ 

shuttle.zeroO; 
shuttle2.zen)0; 
shuttle.add_axisO; 
set eye(0.0,0.0,8); 

} 

//angl ® 0; 

//ang2 * 0; 

//ang3 = 0; 
axisl = 0; 
axis2= 1; 
axis3 * 2; 

NO_GO = 0; 
duration - 0.0; 
show axis-0; 

GO=”0; 
theta = 0; 

> 

} 

> 

inain_windowO; 

ifl:GO=-*l) 

{ /^reparation for first Euler rotation 
reuse « raise * 0; 
if(angl) 

reuse[axisl]«.3 * (angl / abs(aiigl)); //sets angular velocity for rotation 
rot = angl * (pi /180); 

GCH+; 
go_next = 0; 
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} 

if(G0»“2) 

{//GO » 2 animates shuttUe for first rotation 
if(go next) //stop shuttle for second rotation 
{ 

GO++; 

shuttle.assign ang_velocity bc(0,0,0); 

} 

else 

{ 

if((angl > 0 && rot > .3 * read_deltaO) || (angl < 0 && rot < -.3 ♦ read_deltaO)) 
{ //rotation not complete 

shuttle.assign_ang^velocity_bc(reuse); 
rot = rot - reuse[axisl] ♦ read_deltaO; 

} 

else //last part of first rotation 
{ 

if(angl) 

reuse = reuse ♦ (rot / (reuse[axisl] * read_deltaO)); 
shuttle.assign_ang_velocity_bc(reuse); 
go next = 1; 

} 

} 

} 

if(GO = 3) 

{ //prqiaration for second rotation 
reuse = reuse * 0; 
if(ang2) 

teuse[axis2] = .3 * (ang2 / abs(ang2)); 
rot *= ang2 * (pi /180); 

GOH-; 
go next>=0; 

} 

il(GO = 4) //second rotation animation 
{ 

if(go_next) //stop shuttle and go to third rotation 

{ 

GCH-I-; 

shuttlc.assign_ang_velocity_bc(0,0,0); 

> 

else 

{ 

il((ang2 > 0 && rot > .3 * read_deltaO) || (ang2 < 0 && rot < -.3 * read_deltaO)) 
{// rotate at normal rate 

shuttle.assign_ang_velocity_bc(reuse); 
rot = rot - reuse(axis2] * read_deltaO; 

} 

else //last rotation 

{ 

if(ang2) 

reuse = reuse * (rot / (reuse(axis2] * read_deltaO)); 
shuttle.assign_angjvelocity_bc(reuse); 
go next = 1; 

} 
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} 

y 

i£[GO"5) //^rep fbr third ix^on 

{ 

reuse »reuse • 0; 
if(ang3) 

teuse[axis3] ».3 • (ang3 / abs(ang3)); 
rot = ang3 ♦ (pi /180); 

GCH-H; 
go next = 0; 

} 

il(GO=°6) //third rotation animation 

{ 

if(go next) //third rotations complete 

{ 

shuttle.assign_ang_velocityJbc(0.0,0); 

GOH-; 

/^reparation for quaternion rotation 

theta = 2* acos((shuttle.teturn_orientationO)[0]); //calculate theta 
theta_dot ” thcta/5.0; 

q(0] = (shuttlc.retum_orientationO)(ll; //construct q 

qil] > (shuttle.retum_orientation0)[21; 

q(2j (shuttle.retum_orientation0){3i; 

lamda » q * (l/sin(theta/2)); //calculate lamda, axis of rotation 

Iamda.normalizeO; 

av lamHn * theta_dot; //angular velocity required for quaternion rotation 
shuttle2.assign_ang_veloci^av); 

//calculation for axis of rotation display 
reuse(0]« 0.0; 
reusejlj = 1.0; 
reuse[2i = 0.0; 

reuse lamda ^ reuse; //reuse is now the axis of rotation to for the cylinder 
//the cylinder will represent the axis for the shuttle's quaternion rotation 
rot = acos(reuse * lamda); //amout of rotation required for cylinder 
reuse2[0] = 1.0; 
reuse2[l] = 0.0; 
reuse2i2j = 0.0; 

xrot = acos(reuse2 ** reuse); //angle made with x axis 
teuse2[0] = 0.0; 
teuse2|lj -1.0; 
reuse2(2j = 0.0; 

yrot - acos(teuse2 * reuse); //angle made with y axis 
reuse2[0] * 0.0; 
teuse2[l] = 0.0; 
reuse2[2] -1.0; 

zrot» acos(teuse2 * reuse); //angle made with z axis 
teuse_q.set(xtot,yrot,zrot,Tot); //calculate quaternion for orientation of cyilender 
(tylinder.zeroO; 

^linder.assign_orientation(reuse_q); 
cylinder.assign_size(.05,8,.05); 
^Iinder.assign_location(shuttle2.tetum_location0); 
show_axis 1; 

duration » S.O; // sets time for quaternion rotation 

} 
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else 

{ 

if((ang3 > 0 StA rot > .3 * read deltaO) II (ang3 < 0 && rot < -.3 * Tead_deltaO)) 
{ 

shuttle.assign_ang_veIocity_bc(tetise); 
rot - rot - teuse(axis3] * r^_deltaO; 

} 

else 

{ 

if(ang3) 

reuse ~ reuse * (rot / (ieuse(axis3] * read_deltaO)); 
shuttle.assign_aiig^velocity_bc(reuse); 
go_next “ 1; 

} 

} 

} 

if(GO » 7 && show q) //animate quaternion rotation 

{ 

if(duration > 0) 

{ 

shuttle2.update_state0; 
duration = duration - read_deltaO; 

} 

else 

{ //move shuttle together to eliminate parallax problem 
if((shuttle.retum location0)(0] < 0) 

{ 

shuttle.assign_location((shuttle.r^um_IocationO)[0] + 0.02,0,0); 
shuttle2.assign_location((shutde2.tetum_locationO)lO] - 0.02,0,0); 
cylinder.assign location(shuttle2.tetuni locationQ); 

> 

f 

} 

il(show_axis && show_q) cylinder.displayQ; 

shuttle.update_state_tk40; 

shuttle.display0; 

if(show q) 8huttle2.display0; 

} 

} 
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B. GYROSCOPIC 

#include "basc-H" 

^include <stdiib.h> 

voidmainO 

{ 

int secdon * 0, bypass * 0, histoiy = 0; 

int NO_GO = 0. go_iiext “ 0, bmx = 0, moy = 0, moz = 0; 

vectorSD angular_velocity, inertia, size(l,l,l), *old_ang_velocity = new vector3D[3001; 

double duration, step * 0.0, am, ami, am2, am3, time - 0.0. mag = 1.0; 

double elapsed_time = 0.0, total_time = 0.0; 

int mx “ 0, my « 0, GO * 0, obj = 1, x = 0, y = 0, z = 0; 

initializeO; 

initialize_meou0; 

init_control_window0; 

main_window0; 

rigidjbody cube(l), ball(2), cylinder(3), shuttle(50); 

rigid_bo(lty frame(100), rcuse_bod>'; 

reusejbo(fy.assign_shape(ball.retum_shape0); 

reuseJbo(fy.assign_typ^2); 

reusejbody.compute_inertia0; 

set_target(0.0,0.0.0.0); 

srt_eye(0.0.0.0,10); 

s«_time0; 

while (section != 99) 

( 

section « queue_test0; 

sct_delta0; 

viewO; 

//gyro control window data 

gyro_coiitrols(x,y,z,obj,sizc,mox,moy,moz,time,mag,elapsed_time,total_time); 
if(bypass > 0 && bypass < 4) //when mouse has been activated this delays next input 4 cycles 
bypass++; 
else 

bypass = 0; 

(((section > 99999) 

{ 

mx = section / 100000; //decode mouse x coordinate 
my = section - (mx * 100000); //decode mouse y coordinate 
if(!bypass) 

{ 

bypass »1; 

//Size Decrease 

i^mx > 113 && mx < 129) 

{ 

if(my > 936 && my < 953) 
if(size[0] > 0.15) 

sizejo] = size(0] - .1; 
if(my > 923 && rny < 937) 
i((size[l] > 0.15) 

size[l] ■■ size[l] - .1; 
if(my > 909 && my < 924) 
if(size[2] > 0.15) 
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size[2] = size[2] - .1; 
reuse_bo(fy.assign_size(size); 
reuse_body.compute inertiaQ; 

> 

// Size Increase 

if(mx > 165 && mx < 182) 

{ 

ifimy > 936 && my < 953) 
size(0] = size(0] + .1; 
if(my > 923 && my < 937) 
size! 1] = size! 1] + .1; 
if(my > 909 && my < 924) 
size[2] = size[2] + .1; 
reuse_body.assign_size(size); 
reuse_body.compute_inertiaO; 

} 

// Angular Velocity Magnitude Decrease 
if(mx > 193 && mx < 209) 

( 

if(my > 936 && my < 953) 

X-; 

if(my > 923 && my < 937) 

y-; 

if(my > 909 && my < 924) 
z~; 

} 

// Angular Velocity Magnitude Increase 
if(mx > 245 && mx < 262) 

{ 

if(my > 936 && my < 953) 

X++; 

if(my > 923 && my < 937) 

y-H-; 

if(my > 909 && my < 924) 

Z-H-; 

} 

// Moment Magnitude Decrease 
if(mx > 381 && mx < 395) 

{ 

if(my > 936 && my < 953) 

if(time > 0.05) time = time -. 1; 
if(my > 909 && my < 924) 
mag = mag / 10.0; 

} 

// Moment Magnitude Increase 
if(mx > 446 && mx < 463) 

{ 

if(my > 936 && my < 953) 
time - time + .1; 
ifl[my > 909 && my < 924) 
mag = mag * 10.0; 


// Duration & Magnitude Decrease 
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ii(inx > 287 StA mx < 301) 

{ 

if(my > 936 A& my < 953) 
mox-; 

if(my > 923 && my < 937) 
moy-; 

if(my > 909 && my < 924) 
moz—; 

} 

// Duration & Magnitude Increase 
ifimx > 339 && mx < 356) 

{ 

ifKmy > 936 A& my < 953) 
mox++; 

if(my > 923 && my < 937) 
moy++; 

ifl[my > 909 && my < 924) 
moz-H-; 

} 

//Select Shape 

if(mx > 19 && mx < 103) 

{ 

if(my > 936 && my < 953) 

{ 

obj = 1; 

feuseJbody.assign_shape(balI.return_shapeO); 

ieuse_body.assign_typ^2); 

reuse Ixxfy.compute inertiaO; 

} 

if(my > 923 && my < 937) 

{ 

obj-2; 

reuse_body.assign_shape(cube.retum_shapeO); 

reuseJbody.assign_type(l); 

reuse body.compute_ineitiaO; 

} 

if(my > 909 && my < 924) 

{ 

obj » 3; 

reuseJbody.assign_sh^(cylinder.retum_shapeO); 

reuse_body.assign_typ^3); 

teuse_body.oompute_inertiaO; 

} 


// Bo(fy moment button selected 

if(mx > 640 && mx < 707 && my > 890 && my < 959 && !NO_GO) 

{ 

GO = 21; 
duration = time; 
step = 0.0; 
el^sed time = 0.0; 

} 

// Inertial Moment button selected 
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if(inx > 740 && mx < 813 && my > 890 && my < 959 && !NO GO) 

{ 

GO = 31; 
duration - time; 
step * 0.0; 
elapsed time = 0.0; 

} 

// Spin Up button selected 

iHmx > 840 && mx < 907 && my > 890 && my < 959 && !NO_GO) 

{ 

GO=l; 

NO GO = 1; 

} 

if(GO = 2) 

NO_GO = 0; 

// RESET button selected 

if(mx > 940 && mx < 1007 && my > 890 && my < 959) 

{ 

reuseJbody.zeroQ; 

reuse body .assign size(size); 

x=or 

y = 0; 

2 = 0; 
mox = 0; 
moy = 0; 
moz = 0; 

NO_GO = 0; 
step = 0.0; 

} 

} 

} 

main windowO; 
if(Gd= 1) 

{//spin up selected 

rcusc_body.assign_ang_velocity_bc(x,y,z); 

GO++; 

} 

if(GO = 21) //set moment in body coordinates 

{ 

TcuseJbo<!fy.assign_momentJbc(mox ♦ mag, moy * mag, moz * mag); 
duration = duration - step; 
eliq)sed time = elapsed_time + step; 

} 

if(GO = 31) //setmoment in world coordinates 

{ 

reuse_bo<fy.assign_moment(mox * mag, moy * mag, moz * mag); 
duration = duration - step; 
elapsed_time = elapsed_time + step; 

} 

if((GO = 21II GO = 31) && d'’ <tion < 0) //apply moments 

{ 

duration = 0.0; 

GO = 0; 

NO GO»0; 
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} 

frame-displayO; 

induration > 0 &&duration < stq)) set_delta(duration); //last integration step 
step = reusc_body.update_state_iic45(.000001); 
totd_tinie = tota]_tinie + step; 
teuse_body.displayO; 

anguIar_velocity * rcusc_body.retum_ang_velocity_bcO; 

inertia » reuse_body.ietum_inertiaO; //calculate angular momentuin 

ami « angular_velocity[0] * inertia[0]; 

am2 »angular_velocity{l] * inertia(lj; 

am3 = aiiguIar_velocity[2] * inertiallj; 

am « sqrt(aml * ami + am2 * am2 + am3 * am3); //angular momentum 
stat_controls(angular_velocity[0],angular_ve]ocity[ 1 ].angular_velocity[2], 
am,teuse_body.retum_massO, (reuse_body.retum_inertiaO)[0], 
(reuse_body.return_inertiaO}11J, (reuse_body.retujm_ineTtiaO)[2}, 
old_ang_velocity); //display control window and stats 
//old_ang_velocity is a history of the angular velocities for the last 300 cycles. Used for graph 
old_ang_velocity(histoty] = reuse_body.retum_an£_velocity_bcO; 
history = (history + 1) % 300; 


C FRAME OF REFERENCE 

#include "base.H” 

^include <stdlib.h> 

void mainO 

{ 

int section = 0, bypass = 0. vievel = 0. alevel = 0. go_next = 0, i; 

vector3D lvelocity(6], Iposition(6], lang_velI6]; 

int mx = 0. my “ 0. GO = 0, maxjevel = 1, vaxis{6]; 

initializeO; 

initialize_menu0; 

init_control_window0; 

main_windo\v0> 

rigid_body level[6], 11(31), 12(32), 13(33), 14(34), 15(35); //cubes that represents different levels 
(level! l]).assign_shape(ll .retum_shapeO); 

(Ievell2j).assign_sliape(12.retum_shape0); 

(level[3 ]).assign_shape(13 .retum_slu^O); 

(level(4j).assign_shape(14.retum_shape0); 

(level|5j).assign_shape(15.retum_shapeO); 

set_target(0.0,0.0,0.0); 

setjimeO; 

bypass = 0; 

GO = 0; 
vievel = 0; 
alevel” 1; 
maxjevel ” 1; 
for(i^;i<6;i-H-) 

{ 

(level[i]).zetoO; 

Oevel[ii).add_axisO; 

(level[i]).assign_size(.2); 

lposition[i] ” lposition(i] * 0; //location of each level 
lvelocity(i] = lvelocity(i] • 0; //velocity of each level 
lang_yel|i] = lang_vel(i] * 0; //angular velocity of each level 
vaxis[i] ” -1; // viewing axis for each level 

} 

(level[0]).assignjocation(0,0,10); //level 0 is the inertial fiame of reference 
vaxis[0] = -3; 
while (section != 99) 

{ 

section * queuejestQ; 
set_delta0; 

fiame_controls(vlevel, alevel, lvelocity[alevel], ]position[alevel], 
lang^vel(alevel], vaxislvlevelj); //control window 
if(section > 99999 && !byi^) 

{ 

bypass = 14; 

mx = section /100000; //mouse x coordinate 
my ” section - (mx * 100000); //mouse y coordinate 
//Selea Viewing Level 
if(mx > 20 &A mx < 96) 

{ 

if(my > 922 && my < 940) 
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vlevel = 0; 

> 909 A& my < 923) 
vlevel * 1; 

if(my > 8% &&, my < 910) 
vlevel “ 2; 

if(my > 883 && my < 897) 
vlevel ■ 3; 

if(my > 870 && my < 884) 
vlevel = 4; 

ii(my > 857 &ft my < 871) 
vlevel “ 5; 

} 

//Select level for assignment of attributes 
if(mx > 100 &&, mx < 156) 

{ 

if(my > 909 && my < 923) 
alevel = 1; 

if([my > 896 && my < 910) 
alevel = 2; 

if(my > 883 &&. < 897) 

alevel = 3; 

if(my > 870 &A my < 884) 
alevel = 4; 

if(my > 857 A& my < 871) 
alevel = 5; 

if(alevel > maxjevel) 
maxjevel = alevel; 


//Decnax linear velocity 
if(mx > 261 && mx < 276) 

{ 

if(iiiy > 909 && my < 923) 

(lvelocity[alevel])[0] = (lvelocily[alevell)[01 - .1; 
if(my > 896 && my < 910) 

Ovelodty[alevel])(l] = (Ivelocitylalevel])!!] - .1; 
if(my > 883 &A my < 897) 

(lvelocity[alevd])(2] = (lvelocily[alevel])[2] - .1; 
(level{alevel]).assign velocity(lve]ocity[aleve]]); 

} 

//Increase linear velocity 
if(mx > 314 && mx < 329) 

{ 

if(nty > 909 && my < 923) 

(lvelocity(alevel])(0] = Gvelocity[alevell)[0] + .1; 
ii(my > 896 && my < 910) 

(lvdocity[alevel])[l] = GveIocity[alevel])[l] + .1; 
ifCniy > 883 ^ nty < 897) 

(lvelocity[aleveI])[2] * (lvelocity(alevel])(2] + .1; 
GeveIlalevd]).assign_velocity(lvdocitylalevel]); 

} 

//Decrease magninide of position 
if(mx > 347 &A mx < 362) 

{ 
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if(my > 909 &A my < 923) 

(lposition[alevel])[0] (lpositionfalevel])(0] - .1; 
if(my > 896 &A my < 910) 

(Iposition(alevel])[l] = (lposition(aleveI])[l] - .1; 
if(my > 883 && my < 897) 

0position(alevel])[2] =■ (lposition(alevel])[2] • .1; 

//holdeil conatains the location in local coordinates 

(level(alevel]).assign_holderl(lposition[alevel]); 

update location in world coordinates 

(level[ 1 ]).assignJocation((level[ 1 ]).retum_holder 10); 

for(i*2;i<6;i++) 

(level[i]).assign_location((levelIiJ).retum_holdcrlO+(levcl[i-l]).retum_locationO); 


//Increase Magnitude of position 
if(mx > 400 && mx < 416) 

{ 

if(my > 909 && my < 923) 

(lposition(alevel])[0] = (lposition[alevel])[0] + .1; 
if(my > 896 &A my < 910) 

(lposition[alevel])[l] = (lposition[alevel])[l] + .1; 
if(my > 883 && my < 897) 

0position[alevel])(2] = (lposition{alevell)[2] + .1; 
(level(alevel]).assign_holderl(ipositionIalevel]); 

Oevel [ 1 ]).assign_location((level( 1 ]).retum_holder 10); 
for(i“2;i<6;i++) 

(Ievel(il).assign_location((level[il).rctum_holderl0+Ocvel[i-Il).retum_location0); 


//Decrease magnitude of angular velocity 
if(mx > 433 && mx < 449) 

{ 

if(my > 909 && my < 923) 

0ang_vel[alevel])10] = (lang_veli JlO) - .1; 
if(my > 896 && my < 910) 

(lang_vel[alevel])[l] = (iang_vel[alevel])[l] - .1; 
if(my > 883 && my < 897) 

(lang_vel(alevel])[2] = (lang_vel[alevel])[2] - .1; 
(level[alevel]).assign_ang^velocity_b^ang^vel|alevel]); 

} 

//Increase Magnitude of angular veloci^ 
if(mx > 486 && mx < S02) 

{ 

if(my > 909 && my < 923) 

(lang_vel[alevel])[0] = (lang_vel[alevel])[0] + .1; 
if(my > 896 && my < 910) 

(Iang^vel(alevel])[l] = (lang_vel(alevel])[l] + .1; 
if(my > 883 && my < 897) 

(lang^vel[alevel])[2] = (lang^vel(alevel])[2] + .1; 
Oevel[alevel]).assign_ang_velocity bc(lang_vel[alevel]); 

} 

if(vlevel) 

{ 

//Select Negative axis for viewing of scene 
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if(inx > 526 &A mx < 355) 

{ 

if(my > 909 &A my < 923) 
vaxisfvlevel] = -1; //-x axis 
if(my > 896 &A my < 910) 
vaxis[vlevel] = -2; //-y axis 
if(my > 883 && my < 897) 
vaxis(vlevel] = -3; //-z axis 

> 

//Select Positive axis for viewing of scene 
if(mx > 554 Si& mx < 583) 

{ 

if(my > 909 &A my < 923) 
vaxis[vlevel] = 1; //xaxis 
if(my > 896 && my < 910) 
vaxisfvlevel] = 2; //y axis 
if(my > 883 && my < 897) 
vaxisfvlevel] “3; //z axis 

} 

> 

// GO iMitton selected 

ififmx > 740 && mx < 807 && my > 890 && my < 959) 

{ 

GO»l; 

> 

// RESET button selected 

tffmx > 840 A& mx < 907 && my > 890 && my < 959) 

{ 

GO = 0; 
for(i“l;i<6;i-H-) 

(levelfi]).assign_liolderl(lpositionfi]); 

Gevelf 1 ]).assign_location((le^f l]).ietum_holder 10); 
for(i=2;i<6;i++) 

(lcvclfi]).assign_location(flevelfi]).retum_holderlO + 
(levelfi*l]).ietum_locationO); 

} 

} 

main windowO; 
if(Gd) 

{ 

(levelfl]).update_stateO; 

for(i=2;i<6;i++) 

0eveifi]).attaclied body update(levelfi-l]); //integrate frames 

} 

vicw((levelfvlcvel]).rctum_orientationO, Ocvelfvlevel]).ictum_location0, vaxisfvlevel]); 
for(i»l;i<max_lcvel + l;i-H-) 

(levelfi]).display0; //view frames 
if (bypass) bypa^; 

} 

) 
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APPENDIX D 


Rigidjbody Code 


A. HEADER FILE 
#ifiidefRIGID BODY H 
^define RIGID_BODY_H 
#uiclude <inath.h> 

#iiiciude “vector3D.H" 

#iiiclude 'quaternion.H*' 

#inclu(te ”graphics.H" 

#include "time.H" 

^include "matrixSxS.H" 

#uiclude "constants.H" 

class rigid_body 

( 

double mass; 

vector3D *Iocation; 

vector3D velocity; 

vector3D acceleration; 

vector3D force; 

quaternion orientation; 

vector3Dang^velocity; //bodycoordinates 

vector3Dang_acceleiation; //bodycoordinates 

vector3D moment; //body coordinates 

matiix3x3 inertia; 

vector3D size; 

double sui£u:e_area; 

OBJECT *shiq>e; 
int display_axis; 
int *display_shape; 

typejbody; 
vector3D holderl; 
vector3D liolder2; 
quaternion holder3; 

public: 

void compute_ineitiaO; 

rigidJbodyO; 

rigid_body(int); 

rigid_body(char*); 

void assign_mass(double); 

void assign_size(double, double, double); 

void assign_size(double); 

void assign_size(vector3D); 

void assign_su]’&ce_aiea(double); 

void asagn_ineitia(double, double, double); 

void assign_oiientation(double, double, double, double); 
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void as8ign_orieiitation(quateiTUon); 
v<^ assign_sIuq)e(OBJECT*); 
void assign_typ^int); 

void assign_holderl(dotd>le. double, (knible); 
void assign_holder2(d(»ible, double, double); 
void assign_holder3(double, double, double, double); 
vmd assign_holderl(vector3D); 
void assign_holder2(vector3D); 
void assign_holder3(quateniion); 

// Assign values to the items using doubles in world coordinates 
vmd as$ignJocation(double, double, double); 
void assign_vdocity(double, double, double); 
void assign_acoeIeration(double, double, double); 
vdd assign_ang_velocity(double, double, double); 
void assign_ang_acoeleration(double, double, double); 
void assign_force(double, double, double); 
void assign_monient(double, double, double); 


// Assign values to the items using vector3D in world coordinates 

void assign_location(vector3D); 

void assign_vdocity(vector3D); 

void assign_acceleration(vector3D); 

void assign_ang_velocity(vector3D); 

void assign_ang_accderation(veaor3D); 

void assign_force(vector3D); 

void assign_moment(vector3D); 

// Assign values to the items using doubles in body coordinates 
vmd assign_vdocityJ>c(doubie, double, double); 
void assign_acceleiation_bc(double, double, dmible); 
void assign_ang_velocityJbc(double, double, double); 
void assign_ang_aocelerationJbc(double, double, double); 
void assign_forceJbc(double, double, double); 
void assign_moment_bc(double, double, double); 


// Assign values to the items using v6ctor3D in body coordinates 

void assign_velocityJbc(vector3D); 

void assign_accelerationJbc(vector3D); 

void assign_ang_velocityJbc(vector3D); 

void assign_ang_acceleiation_bc(vector3D); 

void assign_forceJbc(vector3D); 

void assign_moment_bc(vector3D); 


// Return values of the items world coordinates 

double retum_massO; 

vector3D tetum_inertiaO; 

vector3D ietttm_sizeO; 

vector3D retumJocationO; 

vector3D* r^umJocationj>trO; 

vectoi3D retumjvelocityO; 


71 







vector3D retuni_accelerationO; 
quaternion retura_orientationO; 
vectorSD ietum_ang_velocityO; 
vector3D retum_ang_accelerationO; 
vector3D ietum_forccO; 
vector3D retum_niomentO; 
d(Md>le ietum_surfiice_areaO; 
OBJECT* retuTn_sha^O; 
int retum_typeO; 
vector3D return_holderlO; 
vector3D ietum_holder20; 
quaternion retum_holder30; 


// Return values of the items body coordinates 
vector3D tetum_vdocity_bcO; 
vector3D retum_acceleration_bcO; 
v6Ctor3D retum_ang_velocity_bcO: 
vector3D retum_ang_acceleration_bcO; 
vector3D retum_force_bcO; 
vector3D retum_moment_bcO; 


void displayO; 

void up^te_stateO; 

void update_state_ik40; 

double update_state_rk45(double); 

vjid add.axisO; 

void removc_axisO; 

void attach_eyeO; 

void attach_targetO; 

void attachedJbody_update(rigid_body); 

void zeroO; 

~rigid_bodyO 

{ 

delete shape; 

delete location; 

} 


void set_^e(double, double, double); 
void set_target(double, double, double); 
#endif 
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B. SOURCE FILE 
«ifiKlefRlGID BODY C 
iMefine RIGID_BODY_C 
#include "rigid_body.H" 

void rigid body:;coinpute_inertiaO 

{ 

switch(typejxxfc') 

{ 

case 1; //Block 

ineitialO]» mass * ((sizell] * size[l)) + (size{2] * size|2])) /12.0 
ineitia^j» mass * ((sizejoj * size(oj) + (size(2j * size(2])) / 12.0 
inettiais] - mass • ((size(0] * size(O)) + (sizellj * sizejl])) /12.0 
break; 

case 2: //Shpere 

ioertiafO]» mass * .1 • size(l] * size(2); 
iiieitia(4] = mass * .1 * sizejo] • size{2i; 
inettiaisi = mass * .1 * sizejo] * sizellj; 
break; 

case 3; //Cylender 

inettiajo] = mass * ((size[2] • size[2)) / 16.0 
+ (size(lj * size(ll) / 12.0); 
inettia(4] = mass * size(0] • size(2] / 8.0; 
inettiais] = mass • ((size(0] * size[0]) /16.0 
+ (size[l] • sizefl]) /12.0); 

break; 


> 

} 

void rigid body ".assign type(intt) 

{ 

type body = t; 

} 

//Defiiult Constructor 
rigid_bo(ty::rigid bodyQ 
{ 

location - new vector3D; 
mass >= 1000.0; 
size(0] ® 1.0; 
sizejl] = 1.0; 
size(21 = 1.0; 
sur&ce_area = 1.0; 
shape = NULL; 
typejbody = 0; 
di^lay_axis = 0; 
di^3lay_shape == new int; 
*di^lay shape * 1; 

} 
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ri^ bo(fy;:rigid body(char* off_file) //Constructor using name of off file 

{ 

location * new vector3D; 

mass= 1000 . 0 ; 

size(0] = 1.0; 

sizejl]» 1.0; 

size|2] * 1.0; 

surfece_arca= 1.0; 

shape = read_object(off_filc); 

rea^_object_for_display(shape); 

QT>e_body = 0; 
display_axis = 0; 
display_shape = new int; 

*display_shape « 1; 

} 

//Constructor that uses predefined shapes 
rigid body;:rigid body(int n) 

{ 

location = new vector3D; 
mass = 1000.0; 
size(0] = 1.0; 
size|lj = 1.0; 
si 2 e (21 == 1.0; 
surf^_area- 1.0; 
switch(n) 

{ 

case 100: 

shape = tead_object("firame.ofr); 

typejbody *T00; 

break; 

case 1; 

shape = read_object("cubc.off'); 
mass = 1000.0; 
inertia[0] = 166.7; 
inertia[4j = 166.7; 
inertia[8] = 166.7; 
lypc_body * 1; 
surface_area = 3.0; 
break; 

case 2: 

shape = read_object("^here.ofir'); 

type_bo^ =» 2; 

mass = 1000.0; 

inertia[0] = 100.0; 

inertia[4j = 100.0; 

inettia[8] = 100.0; 

surfiKX_area = .5; 

break; 

case 3: 

shape = read_object("cylender.off'); 
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type_body = 3; 
mass * 1000.0; 
iiiertia[0]»145.8; 
ineitiai4i» 125.0; 
iiiertia[8] = 327.2; 
surface_area = 2.0; 
break; 

case 15; 

shape * iead_objcct(*fl5.oflr); 
mass = 19076; 
surlace_area = 5.46; 
type_body “ 15; 
break; 

case 23; 

shape = iead_object("rubber_ban.o:''); 

mass = 100; 

surface_area = .01; 

type_bo<fy = 23; 

break; 

case 31; //Level 1 cube for fiame program 

shape = read_object("l 1 ofiT); 

mass = 100; 

sur£ace_area = .01; 

type_body = 1; 

break; 

case 32; //Level 2 cube for ftame program 

shape = read_object("12.ofi"); 

mass= 100; 

surfece_area = .01; 

type_body = 1; 

break; 

case 33; //Level 3 cube for frame program 

shape = read_object("13.ofr); 

mass= 100; 

sur&ce_area .01; 

type_body= 1; 

break; 

case 34; //Level 4 cube for fiame program 

shape = read_object("14.oflr); 

mass= 100; 

sur&ce_area= .01; 

type_body = 1; 

break; 

case 35; //Level 5 cube for firame program 
sluy>e = rcad_object("15.oflr); 
mass - 100; 
sur£ace_area = .01; 
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type_body = 1; 
break; 

case SO : 

shape = rea(l_object("shuttle.ofir); 

type_body = SO; 

mass = 1S70.8; 

inertia[0] = 327.2; 

inertia[4] = 392.7; 

inertia(81 = 327.2; 

break; 

case 90; 

shape = read_object("ground.off"); 

type_body = 90; 

break; 

case 91: 

shape = read_object("floor.ofiP'); 

type_body = 91; 

break; 

default; 

shape = NULL; 
type_body = 0; 
break; 

} 

if (type.body) 

ready_<*ject_for_display(shape); 
display_axis = 0; 
display_shape = new int; 
*display_shape = 1; 


void rigid body::assign shape(OBJECT* o) 

{ 

shape = o; 

} 

void rigid_body;:assign_inass(double n) 

{ 

mass* n; 


void rigid_body;:assign_surface area(double n) 

( 

surface_area = n; 

} 

void rigid body::assign size(double x, double y. dc'jble z) 

{ 

size[0] = x; 
size[li = y; 
size[2j = z; 
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} 


void rigid body:;assign_size(double x) 

{ 

size[0]« x; 
sizcilj “x; 
size|2] = x; 


void rigid body::assign_size(vector3D v) 

{ 

size(0] = v[01; 
sizelll=vlll; 
size(21 = vI2]; 


void rigid body:;assign_location(double x. double y, double z) 

{ 

(*location)(0J = x; 

(♦location)! 1] =y; 

(♦location)[2] = z; 

) 

void rigid body;;assign veiocity(double x, double y, double z) 

{ 

velocitylO] = x; 
velodtyllj =y; 
velocity(2] = z; 


void rigid body::assign_accclcration(double x, double y, double z) 

{ 

accelerationfO] - x; 
accderationjl] »y; 
acceleration|2j == z; 

} 

void rigid_body::assign_forcc(double x, double y, double z) 

{ 

foice[0] = x; 
force(li = y; 
foice(2] = z; 


vmd rigid body::assign_orientation(double x, double y, double z, double w) 

{ 

orientationlO] == x; 
orientationjlj -y; 

(Mientation|2j z, 
orientation[3] w; 

> 
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void rigid body::assign_orientation(quatemion q) 

{ 

orientationfO] == q[0]; 
orientation(l]» q(l]; 
orientationj2]» q(2); 
ori^^ntation(3] = q(31; 

} 

void rigid body:;assign_ineitia(double x, double y, double z) 

{ 

ineilia[0] =x; 
inertia[4j =y; 
inertia[8] 

} 

void rigid_body"assign ang_velocity(double x, double y, double z) 

{ 

vector3D v(x, y, z); 

inatrix3x3 rotation; IN must be transformed from world to bod>' coordinates 
rotation.DCM_world_to_body(orientation); 

V = rotation ♦ v; 
ang_velocity[0] = v[01; 
ang^velocityjll = v(li; 
ang_velocity|2) = vI21; 

} 

void rigid body-assign ang_acceleration(double x, double y, double z) 

{ 

vector3D v(x, y. z); 

matrix3x3 rotation; IN must be transformed from world to body coordinates 
rotation.DCM_world_to_body(orientation); 

V = rotation * v; 
ang_acceleration[0] = v[01; 
ang_acceleration( 1] * vl 1); 
ang_acceleration|2j = v[2j; 

} 

void rigid body::assign_moment(double x, double y. double z) 

{ 

vector3D v(x, y. z); 

matrix3x3 rotation; IN must be transformed from world to body coordinates 
rotation.DCM_world_toJ)ody(orientation); 

V = rotation • v; 
momentlO] =v[0]; 
moment[lj ^v[lj; 
nioinent(2] = v[2]; 


void rigid body:;assign_holderl(double x, double y, double z) 

( 

hoiderl(0] = x; 
holder 1 [ 1] «y; 
holderl|2j = z; 

> 
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void rigid body;:assign hoIder2(double x, double y. double z) 

{ 

holder2[0] = x; 
holder2[l] -y; 
holder2[2i = z; 


void rigid_body;;assign_holder3(double x, double y, double z, double w) 

{ 

holders (0] = x; 
holder3[li = y; 
holder3[2i = z; 
holderSp] = w; 


void rigid body;:assign velocity_bc(double x, double y, double z) 

{ 

vectorSD v(x, y, z); 

matrix3x3 rotation; IN must be transformed from body to world coordinates 

rotation.DCM_bodj_to_world(orientation); 

v = rotation * v; 

vclodtyfOJ = v[0]; 

velocity! 1] = vlli; 

velocity(2] = vpj; 


void rigid body;:assign acceleration bc(double x, double y, double z) 

{ " 

vectorSD v(x, y, z); 

matrix3x3 rotation; IN must be transformed iirom body to world coordinates 
rotation.DCM_body_to_world(orientation); 

V = rotation * v; 
accelerationfO] = v[0]; 
acceleration! 1] = v!li; 
acceleration!2! = v[2!; 

) 

void rigid_body::assign_force_bc(double x. double y, double z) 

{ 

vectorSD v(x, y, z); 

matrix3x3 rotation; IN must be transformed firom body to world coordinates 
rotation.DCM_body_to_woTld(orientation); 

V = rotation * v; 
foroe[0] * v[01; 
forcc[l) = vfl]; 
force[21 = vl2]; 


void rigid_bo(fy;:assign ang_velocity bc(dotd)le x, double y, double z) 

{ 

ang_velocity[0] = x; 
aiig_vdocity!li = y, 
aiig_vdocity!2j« z; 
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} 


void rigid_body "assign ang_acceleration bc(double x. double y, double z) 

{ 

ang_acceleration{0] - x; 
ang_acceleration[l] = y; 
ang^acceIeration(2j» z; 

} 

void rigid_body"assign nioment_bc(double x, double y, double z) 

{ 

inoment[0] == x; 
moment[l] = y; 
momentllj = z; 


void rigid_body"assign_location(vector3D v) 

{ 

(*location)(0] = vfOJ; 

(*location)(ll = vil); 

(*localion)I21 = vI21; 

} 

void rigid body:;assign velocity(vector3D v) 

{ 

velocity[0] =vlO]; 
velocity(lj = vllj; 
velocity[2j = vi2]; 

} 

void rigid body::assign acceleration(vector3D v) 

{ 

accelerationfO] = v[0]: 
acceleration! ij = v[li; 
acceleration!2j = v[2]; 

} 

void rigid_body::assign_force(vector3D v) 

{ 

forceI01 = vI0]; 
force(ll=v[lJ; 
forcei2i = v[2i; 

} 

void rigid bo(fy;;assign ang_velocity(veaor3D v) 

{ 

inatrix3x3 rotation; IN must be transformed from world to body coordinates 
rotation.DCM_world_toJbody(orientation); 

V = rotation • v; 
ang;_vdocity[0] = vIOJ; 
ang_vdocity(li-v[l]; 
ang_vdocity[2] = v[2]; 
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void rigid body:;assign_ang_acc»leration(vector3D v) 

{ 

inatrix3x3 rotation; IN must be transformed from world to body coordinates 
rotation.DCM_world_to_body(orientation); 

V • rotation * v; 
ang_acceleration(0] = v(0); 
ang^accelerationi 1 ]» v| 1 j; 
an£^acceleration[2] - v(2]; 

} 


void rigid body:;assign moment(vector3D v) 

{ 

matrix3x3 rotation; IN must be transformed from world to body coordinates 
rotation.DCM_world_to_body(orientation); 

V “ rotation * v; 
moment(0] = v(0]; 
moment|lj = vjlj; 
moment^] = v[21; 

} 


void rigid body::assign_holderl(vector3D v) 

{ 

holderl = v; 

} 

void rigid body:;assign holder2(vector3D v) 

{ 

holder2 = v; 

} 

void rigid bod|y::assign_holder3(quateniion v) 

{ 

holder3 = v; 

} 


void rigid_body;:assign velocity_bc(vcctor3D v) 

{ 

inatrix3x3 rotation; IN must be transformed from body to world coordinates 
iotatioiLDCM_bo(fy_to_world(orientation); 

V = rotation * v, 
vclocitylO] =v(0]; 
vclocity{l] = v|li; 
velocity^] = v^j; 

> 

void rigid body;:assign_acceleration_bc(vector3D v) 

{ 

inatrix3x3 rotation; IN must be transformed from body to world coordinates 
totation.DCM_bo(fy_to_worId(orientation}; 

V = rotation • v; 
aooeleration(0] v(0]; 
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acceleration(l] v[l]; 
acceleration[2j« v[2]; 


void rigid_body "assign foice_bc(vector3D v) 

{ 

inatrix3x3 nrtation; IN must be transformed from body to world coordinates 
rotation.DCMJbody_to_world(orientation); 

V = rotation * v; 
velocity[0] = v[0J; 
velocityil] = v(ll; 
velocity^] = vi2); 


void rigid body:;assign ang_velocity_bc(vector3D v) 

{ 

ang_velocity[01 = v(01; 
ang_veIocity|li =v[l]; 
ang_velocity[2] = v(21; 

} 

void rigid_body;:assign_ang acceleration_bc(vector3D v) 

{ 

ang_acceleration(0] = v(0); 
ang_acceleration(l] = v[l]; 
ang_acceleration[2i = v(2i; 

} 

void rigid body:;assign moment bc(vector3D v) 

{ " 

moment(0] = v[01; 
moment{lj =v[l]; 
momentl2j = vi2i; 

} 

int rigid_bod>’:;rctum_typeO 

{ 

return type_body; 

} 

double rigid body;:retum_massO 

{ 

return mass; 

> 

vector3D rigid bo(fy::retum_ineitiaO 

{ 

vector3D temp; 
temp[0] inertia[01; 
tempi ij meitiai4]; 
temppj«inertialsj; 
return temp; 
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double rigid bo^::retuni surface areaQ 
letura surface area; 

} 

vector3D rigid bo^::ieturn sizeQ 

{ 

return size; 

} 

vector3D rigid body;;retum_IocationO 

{ 

return *location; 

} 

vector3D* rigid body:;return_location_plrO 

{ 

return location; 

> 

vector3D rigid body;;retum velocityO 

{ 

return velocity; 

} 

vector3D rigid body;;return accelerationQ 

{ 

return acceleration; 

} 

vector3D rigid body:;retum forceQ 

{ 

return force; 

} 

quaternion rigid bodv::retum_orientationO 

{ 

return orientation; 

} 

vector3D rigid_body"return ang_velocityO 

{ 

vector3D v; 
inatrix3x3 rotation; 

totation.DCMJbo(fy_to_world(orientation); 
V = rotation * ang_velocity; 
return v; 

} 

vector3D rigid_boc^::tetum ang_accelerationO 

{ 

vector3D v; 
matrix3x3 rotation; 

iotation.DCMJbod|y_to_world(orientation); 





V - rotation * ang_acceleration; 
return v; 

} 

vector3D rigid body::retuni_inomentO 

{ 

vectorSD v; 
inatrix3x3 rotation; 

rotation.DCM_body_to_world(orientation); 

V rotation * moment; 
return v; 

} 

OBJECT* rigid body:;retum_shapeO 

{ 

return shape; 

} 

vector3D rigid body;:retum_holderlO 

{ 

return holder 1; 

} 

vector3D rigid body::retum holderlQ 

{ 

return hoIder2; 

} 

quaternion rigid bo(fy:;retum_holder30 

{ 

return holder3; 

} 

vector3D rigid body-return velocity_bcO 

{ 

vector3D v; 
matrix3x3 rotation; 

rotation.DCM_worId_toJbody(orientation); 

V = rmation • velocity; 
return v; 

} 

vector3D rigid_body::retum acceleration bcQ 

{ 

vector3D v; 
matrix3x3 rotation; 

rotation.DCM_world_to_bo<fy(orientation); 

V rotation * acceleration; 
return v; 

} 

vector3D rigid_bo(fy::retum force bcQ 

{ 


vector3Dv; 





matrix3x3 rotation; 

rotatioii.DCM_world_to_body(orientatiofl); 

V - rotation • force; 

r^umv; 


vector3D rigid body::retum ang_vdocity_bcO 

{ 

return ang_veloGity; 

> 

vectorSD rigid body:;r«um_ang_accderation_bcO 

{ 

return ang^acceleration; 

} 

vectorBD rigid body::retum_nioment_bcO 

{ 

return moment; 

} 


void rigid body .tupdate stateQ //Euler method of integration 

{ 

vector3D gravity(0.0, -9.81,0.0); 
matrix3x3 rotation; 
doable dt = read_deluO; 

ifCgravity statusQ) //check to see if gravity is on, if yes add gravity to force 

{ 

force “ force + (gravity * mass); 

} 

if(air_resistance statusQ) //check to see if air_resistance is on 

{ 

double magnitude; 

vector3D direction * velocity * -1.0; 

direction.normalizeO; 

/formula below is a rough estimate of force due to air resistance 

magnitude » velocity.magnitudeQ * velocity.magnitudeQ * 0.12 * surface_area; 

force - force + (direction * magnitude); 

} 

acceleration » force / mass; 
velocity ~ velocity + (acceleration * dt); 

*location = *location + (velocity ♦ dt) - (acceleration * (0.5 ♦ dt * dt)); 

//Eulers equations of motion cannot be used for this method of integration because the are unstable 

//for qmining bodies, the equations below are used instead. 

ang_acceleration[0]» moment(0] / inertia[0]; 

ang_acceleration[l] > moment! ij / inettia[4); 

ang_acoeleration[2} - moment(2] / inertia[8j; 

angjvdocity = angjvdocity -t- (ang_acceleration * dt); 

orientation.update(ang_velocity, dt); 
orientation.normalizeO; 

//all forces and moments are zeroed after each integratior The user is required to reapply the force 
foroe[0] = 0.0; 
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force(l] > 0.0; 
foreel21 - 0.0; 
iiionient(O) > 0.0; 
momentll] >0.0; 
moinent|2I > 0.0; 

) 

void rigid body::update state rk40 //Runga Kutta forth order integrator 

//Runga Kutta integrator taken from Numerical Reciepes in C by William Press 
double dt > read_delta0; //dt is time step 
double hh “ dt * .5, h6 = dt / 6; 

vector3D ya > ang_velocity, dyma, dyta, yta, dydxa; //required angular velocity variables 
vectorSD yv ® velocity, dyrav, dytv, ytv, ^dxv; //required velocity variables 
vector3D yl > ^location, dyml, dytl, ytl, dydxl; //required location variables 
quaternion y > orientation, dym, dyt, yt, dydx; //required orientation variables 
int i; 

vector3D gravity(0.0, -9.81,0.0); 
matrix3x3 rotation; 

if(gravity_statusO) //check to see if gravity is turned on 

{ 

force “ force + (gravity * mass); 

} 

if(air_resistance_statusO) //check to see if air resistance is tiuned on 

{ 

double magnitude; 

vector3D direction > velocity • -1.0; 

direction.normalizeO; 

magnitude = velodty.magnitudeO * velocity.magnitudeO * 0.12 ♦ surface_area; 
force = force + (direction • magnitude); 

} 

acceleration > force / mass; 

//calculate initial derivitives 
dydxv > acceleration; 
dydxl = velocity; 

dydxa[0] = (moment(0] - (ang_velocity[ll ♦ ang_velocityl2] * (inertia[8] - inertia(4I))) / inertia(0]; 
<fydxa(l] = (momentll] - (ang_velocityioi * ang_velocity|2j * (inertia[0] - inertiaj8]))) / inertia|4]; 
<fydxa[2j = (moment^] - (ang_velocity[lj * ang_velocityioi * (inertia|4] - inertialO]))) / inertia[8]; 
(fydx[0] = •O.S*((orientation(l] * ang_velocity[0y) + (orientation(2] * ang^velocity]!]) + (orientation(3] * 
ang_velocily(2])); 

d{ydx[l] = 0.S*((orientation[0] ♦ ang_velocity(0]) + (orientation[2] • ang_velocity[2]) - (orientation[3] • 
angjvclocity]!])); 

dydx[2] = 0.5*((orientation[0] ♦ ang_velocity[l]) + (orientation[3) * ang_vclocityI0]) - (orientation] 1] * 
ang_velocity(21)); 

dydx[3] = 0.5*((orientation(0] ♦ ang_velocity[2I) + (orientation] 1] • ang_velocity]l|) - (orientation]2] * 
ang_velocity]0])); 

for(i = 0; i < 3; i-H-) // compute first midpoint y values 

{ 

ytji]»y]i] + hh*dydx]i]; 
yta]il = ya]i] + hh * (fydxa]i]; 
ytvlij = yvji] + hh * <fydxv]i]; 
ytlji] > yl]i) + hh * dydxljij; 
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> 

yt[31-y(31 + hh*dy(ixl31; 

//calculate iww set of derivitives 
dytv » acceleration; 

<^“ytv; 

dytlOJ - -0.5*((yt[ll * ytalOl) + (yt[21 * yta(ll) + (yt[3] * ytaI21)); 
dytdl - 0.5*((yt[01 * ytatOJ) + (yt[2] * ytat2I) - (yt[3] * yta(lj)); 
dytdl - 0.5*((yt[01 * ytall]) + (ytl3] • ytalO]) - (ytll] * ytap])); 
dytdl -0.5*((ytl0] • ytadJ) + (yt[l] •ytall])-(ytI2I • ytalO])); 

<^[0] * (iiionient[0] - 0^11] • yta[2] • (inertia(8) - inenia[4]))) / inenia[0]; 
dytailj * (momentllj - (ytajoj • yta(2j • (inertia(oj - inertials]))) / inertia(4); 
(^(2] = (momentdl - (ytafl] • ytalOJ • (inertia(4) - incrtia[0)))) / inertia[8J; 

for(i >= 0; i < 3; i-H-) //calculate second second midpoint y value 

{ 

ylli]-y(i] + hh*dyt{il; 
ytaji] * ya[il + hh • <fyta(i]; 
ytv(ij = yviij + hh * dytvlij; 
ytl[il=yl[il + hh*dytl(il; 

ytl31 = yI31 + hh*dyt[31; 

//calculate new derivitives 
dymv ■■ acceleration; 
dyml = ytv; 

dym(01 = -0.5*((yt[ll • yta(Ol) + (ytI21 • yta[l]) + (yt[31 • yta[2])); 
dymin = 0.5*((yt[01 • ytalO)) + (yt(2) • yta[21) • (ytd) * ytall))); 
dyml2) = 0.5*((ytl01 • ytall)) + (ytl3) • ytalO)) - (ytll) • ytad))); 
dyml3) = 0.5*((ytl0) • ytad)) + (ytll) • ytall)) - (ytd) * ytalO))); 

^rmalO) - (motnentlO) - C^ll] • ytal2) • (inettial8) • inertial4]))) / inertia)0); 
(tymall) = (momentll) - (ytaioj • ytal2) ♦ (ineitiaioj - inertiaj8)))) / inertia^); 
dyinal2] = (inomentl2] - (ytajl j • ytajo] • (ineitial4) * inertiajo]))) / ineniajs]; 

for(i = 0; i < 3; i++) //calculate end point y values 

{ 

ytlil*yl«l + dt*dlymli); 
ytali] “ yali) + dt • dymali); 
ytvli) * yvli) + dt • t^vli); 
ytlli) = ylli) + dt • dymlli); 

} 

ytl31=yl31 + dt*dyml31; 


for(i = 0; i < 3; i++) 

{ 

(^li) = dymli) + dytli); 

^mali] * dymali) + d^li); 

^mvli) = (i^vli) + d^li); 
dymlli] = (f^ti) + dytlli); 

} 

dymp) = dymd] + dytd); 

= acceleration; 
i^“ytv; 

dytlO) -= -0.5*((ytll) • ytalO)) + (ytp) • ytall)) + (ytp) * ytal2))); 
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dytUl - 0.5*((ytl0] • ytalOj) + (yt[2] * yta(2]) - (yt(31 • ytalU)); 
dytl21 = 0.5*((ytI01 • ytallj) + (ytp) * yta[01) - (ytUl • ytal2])); 
dytpi» 0.5*((yt(01 • yta[21) + (ytlM • ytalM) - (yt(21 * ytalO))); 

(^[0] - (moiiient[0] - • yta(21 * (inertia[8] - iaertia(41))) / inertialOJ 

(^(ij = (momentllj - (ytajoj * yta[2i * (inertialoj - iittitia[8]))) / iiienia(4] 
(^(21 = (moment(2j - (ytall) • ytafOl * (iiiertiaI4] - inertialoj))) / ineitia|8j 


for(i ■ 0; i < 3; i++) 

{ 

orientationji] = yji] + h6 * (dydxjij + dytji] + 2.0 * dym(i]); 
ang_veIocity(i] = yajij + h6 * (dydxaji] + dytafi] + 2.0 • dymajij); 

(*location)(i] = ylji] + h6 * (dy^Jij + dytlji] + 2.0 • dymlji]); 
velocityji] = yv[i] + h6 ♦ (dydxvjij + dytvjij + 2.0 * dyinvji]); 

} 

orientation[3] = y[3J + h6 • (dydx(3] + dyt{3] + 2.0 * dyinf3]); 

orientation. normalize(); 

forceJO] = 0.0; 

forccllj = 0.0; 

force[2] = 0.0; 

momentlO] = 0.0; 

momentllj = 0.0; 

tnoment|2j = 0.0; 

} 

double rigid body;:update state rk45(double eps) //Runga Kutta Adaptive Step Integrator 

{ 

//Runga Kutta integrator taken from Numerical Reciepes in C by William Press 
double PRGOW = -0.20, PSHRNK = -0.25, FCOR = .06666666, dt “ read_deltaO; 
double SAFFTY = 0.9, ERRCON = 6.0e-4, xsav = dt, htry = dt; 
double P = ang_velocity(0], Q = ang_velocity(ll, R = ang_velocityl2]; 
double hh » dt * .5, h6 dt / 6, h, dtl, hhl, h61, errmax; 

vector3D ya = ang_velocity, dyma, dyta, yta, dydxa, dysava, ysava, ytempa, ytemp2a; 
vector3D yv = velocity, dymv, dytv, ytv, ^dxv, dysaw, ysaw, ytempv, ytemp2v; 
vector3D yl = *location, dyml, dytl, ytl, dydxl, dysavl, ysavl, ytempl, ytemp21; 
quaternion y = orientation, dym, dyt, yt, dydx, dysav, ysav, ytemp, ytemp2; 
inti; 

vector3D gravity(0.0, -9.81,0.0); 
matrix3x3 rotation; 
ifi[gravity statusQ) 

{ 

force * force + (gravity * mass); 

} 

if(air resistance_statusO) 

{ 

double magnitude; 

vector3D direction = velocity * -1.0; 

direction.nonnalizeO; 

magnitude velocity.magnitudeQ * velocity.magnitudeQ * 0.12 * surface_area; 
force = force + (direction * magnitude); 

} 

acceleration = force / mass; 


foifi = 0; i < 3; i++) 
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ysavli)=y(i]; 
dysav|i] * (fydx(il; 
ysava[i] = ya(ij; 
(tysava[i]» ^dxa(i); 
ysawli] = yvli]; 
dysaw(ij * dydxv(i]; 
ysavl{i] = yl(i]; 
dysavl(i] = (fydxl[i]; 


} 

ysav(31=yl3]; 
dysav(3] = dydx[3]; 
h * htry; 

for(;;) { 
hh = 0.5 ♦ h; 
dtl = hh; 
hhl “dtl *0.5; 
h61 = dti/6.0; 

<fydxv acceleration; 
dydxl = velocity; 

^dxa(0] “ (moment(0] - (ang_velocitylll * ang_velocity(2] * (inertialS) - inertiaI4]))) / inertialO); 
(fydxa[l] = (momentll] - (ang_velocity[0] • ang^velocitylz] * (ineitia|0] - inertia[8]))) / mertia[4]; 
(fydxa(2j = (nioinent(2j - (ang_velocity[l] * ang_velocity|oj * (incrtiai4j - inertia(Ol))) / inertia[8]; 
<fydx(01 = -O.S*((oiientation(l] * ang__velocityl01) + (orienution[2] * ang_vclocity[l)) + (orientational * 
ang_velocityI2])); 

dydx(l] = 0.S*((orientation{0] • ang_velocity(01) + (orientation(2] * ang_velocity[2I) - (orientation[3I * 
ang;_velocity(l])); 

dydx(21 = 0.5*((orientation(01 * ang_veIocity{ll) + (orientation(3] ♦ ang_velocity[0]) - (orientation! 1] ♦ 
ang_velocity(2))); 

dydx(3) = 0.S*((orientation[0] * ang^velocity[2)) + (orientation! 1] ♦ ang^velocity!!]) - (orientation|2] * 
ang_velocityIO])); 


for(i = 0; i < 3; i-H-) 

{ 

ytlil = ysavlij + hhl ♦ dydx!i]; 
ytalij = ysavalij + hhl ♦ dydxa!i]; 
ytvjil = ysaw[i] + hhl ♦ dydxv!i]; 
ytlli] = ysavlfi] + hhl * dydxl!i]; 

} 

ytI31 = ysav!31 + hhl * dydx[3]; 

<^tv = acceleration; 
dytl = ytv; 

dytIO] = -0.5*((ytll) * yta!0]) + (ytI2] • ytall]) + (yt^J • yta!2])); 

dytllj = 0.5*((ytt01 • yta[0J) + (yt[2\ * yta[21) - (yt[3] * ytall])); 

dytpi = 0.5*((ytl01 • ytall)) + (ytp) * ytalO)) - (yt(ll * ytal2))); 

dyt[31 = 0.5*((ytl0I * ytal21) + (ytH) * yta(l]) - (yt(2J * yta!0])); 

dyta[0] = (moment[0] - (yta[l] * yta(2] ♦ (inertia!8] - ineitia!4]))) / inertialO]; 

djtall] * (momentll) - OdalO) • ytal2] ♦ (inertialO) - ineitial8]))) / inertia!4]; 

(^12] = (momentl2] - (ytall) • ytajo) * (inertial4] - inertialO]))) / inertia!8]; 


for(i = 0; i < 3; i++) 
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{ 

ytli) = ysav(i] + hhl * <fyt[ij; 
yta(i] = ysava[i] + hhl * 
ytv(ij = ysaw[i] + hhl ♦ 
ytl[i] = ysavlli] + hhl • dytl[i]; 

} 

yt[31 = ysav[31 + hhl *dytl31; 

dymv = acceleration; 
dyml = ytv; 

dymlOl = -0.5*((ytll] * yta[01) + (yt(2] * yta[l|) + (ytI31 * ytaI21)); 
dymlll = 0.5*((ytl0] * ytalO]) + (ytI21 • ytaI21) - (yt{3) * ytall])); 
dyin(2] = 0.5*((yt[0] * yta(l]) + (yt[3] * ytalO]) - (yt(ll * ytaI21)); 
dym[3] = 0.5*((yt(01 * yta[21) + (yt[l\ * yta[ll) - (yt[21 * yta[0])); 
c^mafO] = (moment[0] - C^[l] * yta[2] ♦ (inertia[8] - inertia[4]))) / meitia[0] 
<fyina[lj = (momentjlj - (yta(0] * ytaP] ♦ (inertia[oj - mertia[8]))) / inertia[4] 
(^m[ia|2j = (moment[2] - (yta[l] * ytaloj ♦ (inertiai4] - mertia[0]))) / inertia(8i 


for(i = 0; i < 3; i++) 

{ 

yt[i] = ysav[i] + dtl * dyin[i]; 
yta[i] = ysava[i] + dtl * dyina(i]; 
ytv[i] =ysaw(il + dtl * (tymv[il; 
ytl(i] =ysavl(i] + dtl * dymlfi]; 

} 

yt[3] =ysav[3] + dtl • dymI3]; 


for(i = 0; i < 3; i-H-) 

{ 

dym[i] = dyin[i] + dytfi]; 

(^ina[i] = dyina[i] + ^1a[i]; 

<fymv[ii = cfymvji] + <^tv{i]; 
dynil[i] = dyinl[i] + dytl[ij; 

} 

<fyin[3] = dyin[3] + ^[31; 

dytv = acceleration; 

<^ = ytv; 

dytIO] = -0.5*((ytll] * ytalOJ) + (yt[21 * ytall]) + (yt[31 • yta[2])); 

dyt[ll = 0.5*((yt[01 * yta{0)) + (yt{2] * ytaI2]) - (ytl31 * yta]!])); 

dytl2] * 0.5*((yt[0] * yta]!]) + (ytl3] * ytalO]) - (ytH] * yta[2])); 

dyt[3] = 0.5*((yt[0] * ytaI2]) + (ytll] * ytall]) - (ytp] * yta[0])); 

dyta[0] = (moment[0] - (yta[l] * yta[2] ♦ (inertia[8] - inertial4]))) / inertia[0]; 

d^[l] = (momentllj - (yta[0] • yta[2] * (meitia[0] - inertialS]))) / inertia^]; 

^ta[2] = (moment[2] - (yta[l] * yta[0] * (inertia[4] - inertia[0]))) / inertia[8]; 


for(i = 0; i < 3; i-H-) 

{ 

ytempli]» ysavp] + h61 * (dydx[i] + dyt[i] + 2.0 ♦ dymp]); 
ytempafi] = ysava[i] -*■ h61 • (tfydxafi] -♦• c^[i] -•- 2.0 * 4^[i]); 
ytemplli] = ysavl[i] + h61 * (^dxl[i] + djytlli] + 2.0 * dyiid[i]); 
ytcmpvli] = ysaw[i] -*• h61 • (dydxvli] -*• dytvli] -•- 2.0 • dymvli]); 
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} 

yteinp[3] = ysav[3] + h61 * (dydx[31 + dytl3] + 2.0 • dyml31); 


//assign new ys for second rk4 
for(i = 0; i < 3; i-H-) 

{ 

y(ij = ytemp{il; 
yali] =ytempali]; 
yl(i]“ytempllij; 
yv[i] = ytempv(i]; 

} 

y[31 = ytemp[31; 

//calculate new set of derivatives 
dydxv = acceleration; 

(fydxl = ytempv; 

(fydx[0] = ■0.5*((yteinp[l] • ytempafO]) + (ytemp{2J * ytempa[l]) + (ytemp[3] * ylempa[2])) 
<fydx(li = 0.5*((yteinp[0] * ytempalO]) + (ytemp[21 * ytempaI2]) - (ylempis] * ytempafl])); 
dydx[2] = 0.5*((yteinpio] * ytempajl]) + (ytempisj * ytenjpa[0]) - (ytemp[l) * ytempal2])); 
cfydxls] = 0.5*((ytenip(0] • ytempa[2j) + (ytempllj * ytempa[l]) - (ytemp[2) * ytempa(Ol)); 
^dxafO] = (momentlO] - (ytempa[l) * ytempa[21 * (inertia[8] - inertia[41))) / inertia(0]; 
(fydxa[lj = (moment[lj - (ytempa[o] * ytempa[2] * (inertia|oj - inertia[8j))) / inertia[4]; 
dydxa[2] = (moment|2] - (ytempajl] * ytempa[01 * (inertia[4] - iner i[0]))) / inertia[8]; 

for(i =» 0; i < 3; i++) 

{ 

ytli] = yl*] + hhl • dydx(i]; 
ytapl = yaji] + hhl • dydxa(i]; 
ytvjij = yvji] + hhl • ^dxv(i]; 
yUti]=yl[il + hhl *dydxl[il; 

} 

yt[3] = y[3] + hhl*dydx[31; 

dytv = acceleration; 

= ytv; 

dytIO] = -0.5*((yt[l] ♦ yta[0]) + (ytI2] • ytallj) + (yt[3] * ytaI2])); 

dytll] = 0.5*((yt[0] * ytalO]) + (ytl2] * ytal21) - (ytI3] * yta]!])); 

dyt[2] = 0.5*((ytI0] * yta[l]) + (yt[31 * yta[01) - (yt[l] * yta[2])); 

dytI3| = 0.5*((yt(0] * yta(2]) + (yt(lj * ytallJ) - (yt(2] * ytafOJ)); 

c^[0] = (moment[0] - (yta[l] ♦ ytai2] * (inertiaI8] - inertia[4]))) / inertia[0]; 

<^|li = (momentjlj - ^lalO] * ytaI2] * (inertiajoj - inertiai8]))) / inertia|4]; 
dyta|2j = (moment|2] - (ytajl] * ytajo] ♦ (inertia[4] - inertiajo]))) / inertiaI8]; 

for(i = 0; i < 3; i-H-) 

{ 

yt[i]=y[il + hhl *dytli]; 
ytali] = yali] -^ hhl • ^tali]; 
ytvji] = yv[i] + hhl • 4lv[i]; 
ytl[i] = yl[ij + hhl • dytl[i]; 

> 

yt[3]=yl3]-Hihl*dyt[3J; 


^miv acceleration; 
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dyml = ytv; 

dyin[0] = -0.5*((ytlll * )ta[0]) + (ytI2] * yta(ll) + (yt(31 * yta[2])); 
dym[ll - 0.5*((yt[01 * yta[01) + (yt[21 * yta[21) - (ytl31 * ytall])); 
dym[21 = 0.5*((yt[0] • ytall)) + (yt(3] • ytalOj) - (yt(lj * yta[2])); 
dyin[31 •= 0.5*((ytl01 • ytal21) + (ytll) • ytallj) - (yt^J * yta|0})); 

(fyma[0] = (moment[0] - (^[1] * yta[21 * (mertia[8] - meitia[4]))) / inertia[0] 
dynia[l] = (inoment[l] - (yta(OJ ♦ yta(21 * (inertia[0] - ineitiafS]))) / inertia[4] 
dyina(2] = (inoinent[2] - (yta[l] • yta(0] * (inertia[4] - inertia[0]))) / ineitia[8] 

for(i = 0; i < 3; i++) 

{ 

yt[il = y[il + dtl * dymli); 
yta[il = ya(il + dtl * dyma[i]; 
ytv|ij = yv[il + dtl * 
ytlli] =ylli] + dtl * dyinl(il; 

} 

ytl3] = y(3] + dtl *dym(3]; 


for(i = 0; i < 3; 1++) 

{ 

dym[i] = dym[i] + dytli]; 

= dyma(i] + 

(^v|ij = d^v[i] + 

(^inl[i] = d^[i] + dytl[il; 

} 

dymPJ = <fymI3J + dyt[3]; 

dytv = acceleration; 
d)4l = ytv; 

dyt[0] = -0.5*((yt(l] * yta(Ol) + (yt[2] * ytall]) + (yt[3] * ytal2])); 

dytll] = 0.5*((ytlO] * ytalOl) + (yt{21 * ytal2]) - (ytp] * yta[l])); 

dyt(2] = 0.5*{(yt[0] * yta[ll) + (ytI3] * yta[0]) - (yt[l] * ytaI2])); 

dytI3] = 0.5*((yt[0] • ytaI2]) + (yt]!] * yta(l]) - (yt(2} * yta[OJ)); 

dyta[0] = (moment[0] - (yta[l] ♦ ytal2] * (inertia|8] - inertia[4]))) / inertia[0]; 

(^[l] = (momentllj - (ytaioj * yta[2] • (inertiajo] - ineitia(8i))) / inertia[4]; 

^ta[2] = (moment(2] - (ytall] * yta[0] * (inertia[4] - ineTtiajo]))) / ineitia[8]; 


for(i = 0; i < 3; i++) 

{ 

ytemp2[i] = y[i] + h61 • (dydx[i] + dyt[i] + 2.0 * dymli]); 
ytemp2a[i] = yap] + h61 * (dydxa(i] + dytap] + 2.0 * dymap]); 
ytemp21[i] = yl[i] + h61 * (dydxlp] + dytlfi] + 2.0 ♦ dyml[i]); 
ytemp2vp] = yvp] + h61 * (dy(k^[i] + dytvp] + 2.0 * dymvp]); 

> 

ytemp2[3] = yt3] + h61 * (dydx[3] + dyt[3] + 2.0 * ^[3]); 

//thrid ik4 run 
dtl=h; 

hhl » dtl • 0.5; 
h61“dtl/6.0; 
dydxv = acceleration; 
ctydxl« velocity; 
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dydxalO] * (moment[0] - (ang_velocity|l] * ang_velocity(2] * (meitia[8] - mertia(4)))) / meitia[0]; 
dydxail] = (moment[l] - (ang_velocity(01 ♦ ang_veIocity(2i * (inertia[0] - ineitia(8]))) / inertia[4]; 
(fydxai2] = (momeiit[2] - (ang^velocityilj * ang_velocityioj * (inertia[4] - ineTtiajo]))) / inertia[8]; 
(fydx[0] = -0.5*((orientation[ll * ang_velocity(01) + (orientation[2] * ang_velocity[l]) + (orientation[3] * 
an£_velocity(2])); 

(fydxlll =0.5*((orientation[01 * angjvelocitylOl) + (orientaUon[2] * ang_velocityl21) - (orientationl3| ♦ 
ang^velocity[lJ)); 

dydx[2] = 0.5*((orientation[0] • ang_velocity(ll) + (orientation[3] * ang_velocity(01) - (orientation! 1] ♦ 
ang_velocity(2])); 

dydx(3] = 0.5*((orientation[0J • ang_vclocity[2]) + (orientation! 1] • ang_velocity!l]) - (orientation!2] * 
ang_velocity!01)); 


for(i = 0; i < 3; i++) 

{ 

yt!i] = ysav!il + hhl * dydx!i]; 
yta(i] = ysavafi] + hhl * dydxa!i]; 
ytv!i] =ysawli] + hhl * cfydxv!!]; 
ytl!i] = ysavUi] + hhl * dydxl!i]; 

} 

ytP] = ysavI3] + hhl * dydx!3]; 

dytv = acceleration; 
dytl = ytv; 

dytlO] = -0.5*((yt!ll * yta!0J) + (yt!2] ♦ yta!l]) + (ytI3] * ytap})); 

dyt!l] = 0.5*((ytI01 * yta!01) + (ytI2] * yta!2]) - (yt!3) * yta!ll)); 

dyt!2] = 0.5*m0] * ytallj) + (yt!31 * yta!01) - (ytHl * yta!2])); 

dytI3] = 0.5*m0] * ytaI2)) + (yt[l] • yta!l]) - (yt[2] * yta!0])); 

dyta!0] = (moment!0] - (ytall] • yta!21 * (inertia!8] - inertia!4]))) / inertia!0] 

^tallj =■ (moment! 1] - (ytajo] * ytapj * (inertialo] - inertia!8j))) / inertia!4] 

^tal2] = (momentpj - (ytallj * ytajoj * (inertia!4] - ineitiajoj))) / ineitia!8] 


for(i = 0; i < 3; i++) 

{ 

yt!i] = ysav!i] + hhl ♦ dyt!i]; 
yta!i] = ysava!i] + hhl * <fyta!i]; 
ytvpl = ysaw[i] + hhl * ^tv(il; 
ytl!il = ysavUi] + hhl * dytl!ij; 

} 

yt!3] = ysavI3] + hhl * dytI3]; 


» acceleration; 

^ml = ytv; 

dymIOl = -0.5*((yt[l] * ytalO]) + (yt(21 * yta[ll) + (ytpi * ytaI2])); 
dym[l) = 0.5*((ytI0J * ytalO]) + (ytI2] • yta[2]) - (ytI3J * yta!l])); 
dym!2] = 0.5*((yt!0] * yu!lj) + (yt!3] * ytalO]) - (yt[l] * ytaI2])); 
dymI3] = 0.5*((ytI0] ♦ ytaI2]) + (yt[l] * yta[lj) - (yt(21 * yta!0])); 

^onalO] = (moment[0] - (}4a|l] * yta|2] ♦ (inertia[8] - ineTtia!4]))) / ineitia!0] 
c^ajli = (moment|lj - (yta[0] * ytapj * (inertialoj - ineitia!8]})) / ineitia!4]; 
(^nmabi = (moment^j - ^[ij * ytajoj * (ineitia|4j - ineitia[oj))) / ineitials]; 


for(i = 0; i < 3; i++) 

{ 

ytp] = ysavp] + dtl ♦ 

ytapl “ysavali) + dtl * djinaliJ; 
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ytvji] = ysaw[i] + dtl * dymv[i]; 
ytl(il “ ysavl[i] + dtl * dyinl[i]; 

} 

yt[3J = ysav[3] + dtl * dym[31; 


for(i “ 0; i < 3; 1++) 

{ 

dymfi] = dyin[i] + dyt[i]; 
dyina[i] = dyina[i] + dytafij; 

<^v[i) = ^TOv(il + c^[il; 
dyinl[i] = dj^U] + dytl[i]; 

} 

dym[31 = dyinI31+ dytI3]; 

dytv = acceleration; 
dytl = ytv; 

dytIO] = -0.5*((yt[ll * yta[01) + (yt[2] * yta[ll) + (yt[3] * yta[2])); 

dyt[ll = 0.5*((yt[0] * yta[0]) + (yt[21 * yta[21) - (yt(31 * ytall])); 

dyt[21 = 0.5*((yt{01 * yta[ll) + (yt[3] * yta[OJ) - (yt[ll * ytaI21)); 

dyt(3] = 0.5*((yt(0] ♦ yta[2]) + (yt[l] * yta[l]) - (yt[2] * yta[0])); 

dyta[0] = (momentfO] - (yta[l] ♦ yta[2] * (ineitia[8] - inertia[4]))) / ineitia[0]; 

dyta(l] = (momentll] - (yta[0] * yXa\l\ * (inertia[0] - inertia[8]))) / inertia[4]; 

dyta|2] = (moinent[2] - (yta[l] ♦ yta[0] * (ineitia[4] - inertia[0]))) / inertia[8]; 


for(i ® 0; i < 3; i++) 

{ 

ytemp[i] = ysav[i] + h61 • (dydx[i] + dytli] + 2.0 * dyin[i)); 
ytempa(i] = ysava[i] + h61 • (dydxa[i] + dyta[i] + 2.0 * dyxnali]); 
ytempl[i] = ysavl(i] + h61 ♦ (dydxl[i] + dytl[il + 2.0 * dyml[i]); 
ytempvp] = ysaw[i] + h61 * (dydxv[ij + dytv[i] + 2.0 * dymv[i]); 

} 

ytemp(3] = ysavI3] + h61 * (dydx[3] + dyt[3] + 2.0 ♦ dym[3J); 

//error determination 
emnax = 0.0; 
for(i = 0; i < 3; i++) 

{ 

ytemp[i] = ytemp2[i] -ytemp[i]; 

if(ernnax < £abs(yt^p[i])) emnax = iabsOtemp[i]); 

ytempa[i] =ytemp2a[i] -ytempa[i]; 

if(ernnax < fiibs(^empa[i])) errmax = fabs(ytempa[i]); 

ytempl[i] = ytemp21[i] -ytempl[i]; 

if(ernnax < £ibs(ytempl[i])) emnax = &bs(ytempi[i]); 

ytempv[i] =ytemp2v[i] -ytempvli]; 

if(ernnax < £abs(ytempv[i])) errmax = fabs(ytempvli]); 

} 

ytcmp(3J = ytemp2[3] - ytemp[3]; 
if(ernnax < &bs0'temp[3])) errmax = fiibs(ytemp[3]); 
errmax /= eps; 
ifi[emuax < 1.0) 
break; 

h = SAFETY * h ♦ exp(PSHRNK*log(emnax)); 
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} 


for(i = 0; i < 3; i-H-) 

{ 

orientation[i] = ytemp2[i] + ytemp[i] * FCOR; 
ang_velocity[i] = yteinp2a(i] + ytempa[i} * FCOR; 
(*location)[i] = ytemp21[i] + ytempl(i) ♦ FCOR; 
velocity[i] = ytemp2v[i] + ytenipv[i] ♦ FCOR; 

} 

orientational =ytemp2[31 +ytemp[31 * FCOR; 

orientation.nonnalizeO; 

foice[0] = 0.0; 

force! 1] = 0.0; 

foice(2]=0.0; 

nu>ment[0] = 0.0; 

moment! 1] = 0.0; 

moment^] = 0.0; 

return h; 

} 

void rigid_body::display0 

{ 

Matrix rt = { 1.0,0.0,0.0,0.0, 

0 . 0 , 1 . 0 , 0 . 0 , 0 . 0 , 

0 . 0 , 0 . 0 , 1 . 0 , 0 . 0 , 

0 . 0 , 0 . 0 , 0 . 0 , 1 . 0 }; 


Matrix scale * { 1.0,0.0,0.0,0.0, 

0 . 0 , 1 . 0 , 0 . 0 , 0 . 0 , 

0 . 0 , 0 . 0 , 1 . 0 , 0 . 0 , 

0 . 0 , 0 . 0 , 0 . 0 , 1 . 0 }; 

pushmatrixO; 

//calculate values for the rotation part of matrix 

rt(0][0] = ((orientation[0] • orientation[0]) + (orientation! 1] * 
orientation!!]) - (orientation!2] ♦ orientation!2]) - 
(orientation!3] * orientation!3])); 

ri!l]!0] = 2.0 * ((orientation!!] * orientation!2]) - (orientationlO] * 
orientation!3])); 

ri!21!01 = 2.0 * ((orientationlO] * orientation!2]) + (orientation]!] * 
orientation!3])); 

ri!0]!l] = 2.0 * ((orientation] 1] * orientation!2]) + (orientationlO] * 
orientationp])); 

rt]!]]!] = ((orientationlO] * orientationlO]) - (orientation]!] * 
orientation] 1]) + (orientation!2] * orientation!2]) - 
(orientation!3] * orientation!3])); 

ri!2]!l] = 2.0 * ((orientation!2] * orientation!3]) - (orientationlO] * 
orientationll])); 

ril0]12] = 2.0 * ((orientationll] • orientadonp]) - (orientationlO] * 
orientationl2])); 

rill]12] = 2.0 * ((orientation!2] * orientation^]) + (orientationlO] * 
orientationll])); 

ril2]12] = ((orientationlO] * orientationlO]) - (orientationll] * 
orientationll]) - (orientationl2j * orientationl2]) + 
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(orientation(3] * orientation[3])); 

//load traxislation part of matrix 
rtpiIOJ = (*location)[0]; 
rtl3]ll] = (*location)lll; 
rtl3]l21»(*location)I21; 

multinatrix(rt); //perform rotataion and translation of rigid_bod>' 

scale[0](0] = sizcIO]; 
scale[l][l] = size[l]; 
scale|2][2i = si 2 e[ 2 ]; 
multmatrix(scale); //scale the rigid_body 

if(display axis St& '*display_shape) //display rigid body's body axes 

{ 

view_axisO; 

} 

if(*display_shape) //display shape if eye is not attached to this rigid_body 

{ 

display_this_object(shape); 

} 

popmatrixO; 

} 

void rigid body;;add axisQ 

{ 

display axis= 1; 

} 

void rigid body:;remove axisQ 

( 

display axis = 0; 

} 

void rigid_body:;attach_eyeO 

{ 

attach_eye toOocation, display_shape); 

} 

void rigid_body;;attach_targetO 

{ 

attach_target to(location); 

} 

void set_eye(double x, double y, double z) 

{ 

set eye to{x, y, z); 

} 

void set_target(double x, double y, double z) 

{ 

set target_to(x, y. z); 

} 


void rigid_bo^::zeroO 
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{ 

vectorSD zcro_vector, 
size(01 = 1.0; 
size[li = 1.0; 
size[2] >= 1.0; 

^location * zero_vector, 
velocity = 2 cro_vector, 
acceleration * zero_vector; 
orientation(0] ° 1.0; 
orientation! 1] = 0.0; 
orientation|2j = 0.0; 
orientation[3j = 0.0; 
ang;_velocity = zero_vector. 
ang_acceleration = zero_vector; 
display axis = 0; 

} 

void rigid body;;attached_body update(rigid_body r) 

{ 

vector3D av; 
niatrix3x3 rotation; 

^location = holderl; //retrieve local position 

update_state0; 

holderl = ^location; //store local position 
rotation.DCM_body_to_world(r.orientation); 

^location = (rotation * (♦location)) + *r.location; //calculate position in world coordinates 
//make adjustment for rotation of frame of reference with respect to the world 
holder2 = (rotation * r.ang_velocity) + r.holder2; 

//holder2 now contains the sum of the angular velocities of all of the previous rigid_bodies 
av = holder2; 

rotation.DCM_world_to_body(orientation); 

av » rotation ♦ av; //transforms av to the calling rigid_body*s bocty coordinates 
orientation.update(av,read_deltaO); //rotates the rigid body to account for rotation of previous frames 

} 

#endif 
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APPENDIX E 


VectorSD Code 


A. HEADER FILE 
#ifhdefVECTOR3D H 
#define VECTOR3D_H 
#include <iostreain.h> 

#include <inath.h> 

class vector3D 

{ 

double x; 
double y; 
double z; 
public; 

vcctor3D0; 

vector3D(double, double, double); 

vector3D(const vector3D&); 

vector3D& operator=(const vector3D&); 

vector3D operator+(const vector3D&); 

vector3D operator*(coast vector3D&); 

double operator*(const vector3D&); //dot product 

vector3D operator*(double); //scalar multiplication 

vector3D operator/(double); //scalar division 

vector3D operator^(const vector3D&); //cross product 

double magnitudeO; 

void normalizeO; 

void nomialize(double); 

friend ostream& operator«(ostream&, vector3D&); 
doubleA operatorQ(int); 

~vector3D0 { } 

}; 

#endif 
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B. SOURCE FILE 
#iflidefVECTOR3D_C 
#defineVECTOR3D_C 
^include "vector3D.H" 

//de&ult constructor 
vectorSD: :vector3D0 
{ 

X * 0.0; 

y = 0.0; 
z = 0.0; 

} 

//constructor using three doubles 
vector3D;;vector3D(double a, double b, double c) 

{ 

x = a; 
y = b; 
z = c; 

) 

//constructor using another vector3D 
vector3D::vector3D(const vector3D& v) 

{ 

x = v.x; 
y = v.y; 
z “ v.z; 

} 

//Assignment operator the function must return a reference to a vector 
//instead of a vector for assignment to work properly 
vector3D& vector3D::operator=(const vector3D& v) 

{ 

x = v.x; 
y = v.y; 
z = v.z; 
return *this; 

} 

//vector addition operator 

vector3D vector3D::operatorf(const vector3D& v) 

{ 

vector3D sum; 
sum.x = v.x + x; 
sum.y = v.y + y; 
sum.z = V.Z + z; 
return sum; 

} 

//vector substiaction 

vectorSD vector3D::operator-(const vector3D& v) 

{ 

vector3D diff; 
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diff.x = X - v.x; 
diff.y -y • v.y; 
diff.z ® z - v.z; 
return difT; 

} 

/Arector dot product 

double vector3D:;operator*(const vectors D& v) 
double dot; 

dot = (v.x * x) + (v.y * y) + (v.z ♦ z); 
return dot; 

} 

//scalar multiplication 

vcctorSD vector3D::operator*(double n) 

{ 

vectorSD mult; 
mult.x = X * n; 
mult.y = y * n; 
mult.z = z * n; 
return mult; 

} 

//scalar division • it is the user responsibility to make sure that n is not zero 
vectorSD vector3D::operator/(double n) 

{ 

vectorSD result; 
result.x = X / n; 

Tesult.y = y / n; 
result.z = z / n; 
return result; 

} 

//vector cross product 

vectorSD vector3D::operator^(const vector3D& v) 

( 

vectorSD cross; 
cross.x = (y ♦ v.z) - (v.y * z); 
cross.y = -((x ♦ v.z) - (v.x * z)); 
cross.z = (x * v.y) - (v.x * y); 
return cross; 

} 

//the « operator is to be used with cout 
oStream& operator«(ostream& os, vector3D& v) 

{ 

os «(double) v.x «", " «(double) v.y « ", " « (double) v.z «"\n" 
return os; 

} 

//allows access to the components of the vectorSD. it must return a reference 
/fin order for assignment to work 
double& vector3D;:q)eratorQ(int n) 
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if (n = 0) 

{ 

return x; 

} 

if(n=l) 

{ 

return y; 

} 

if(n = 2) 

{ 

retiun z; 

} 


} 

//returns the magnitude of the vector 
double vector3D::magnitudeO 
{ 

return sqrt((x * x) + (y * y) + (z * z)); 

} 

//normalizes the vector to one 
void vector3D::normalizeO 
{ 

double m = sqrt((x ♦ x) + (y ♦ y) + (z * z)); 
if(m) 

{ 

X = X / m; 
y = y / m; 
z = z/m; 

} 

} 

//normalize the vector to d 

void vector3D'.'.normalize(double d) 

{ 

double m = sqrt((x * x) + (y * y) + (z ♦ z)); 
if{m) 

{ 

X = d ♦ X / m; 
y = d * y / m; 
z = d * z / m; 

) 

} 


#endif 
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APPENDIX F 


Quaternion Code 


A> BKADER FILE 
#ifhdef QU ATERNION_H 
#define QUATERNION_H 
#include <iostreain.h> 
^include <math.h> 
#include "vectorSD.H" 


class quaternion 

{ 

double qO; 
double ql; 
double q2; 
double q3; 
public; 

quatemionO; 

quatemion(double, double, double, double); 
quatemion(const quatemion&); 
void set(double, double, double, double); 
quatemion& (qwiatoi^const quatemion&); 
quaternion operator+(const quatemion&); 
quaternion operator-(const quatemion&); 
quaternion (^rator*(const quatemion&); 
quaternion operator*(double); 
double& operatorQ(int); 
double magnitudeO; 
void normalizeO; 

quaternion rate_of_change(double, double, double); 

void update(double, double, double, double); 

quaternion ratc_of_change(vector3D); 

void update(vector3D, double); 

friend ostreain& operator«(ostream&, quatemion&); 

~quatemionO { } 


#endif 
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B. SOURCE FILE 
#ifedef QU ATERNION_C 
^define QUATERNION_C 
#include "quaternion.H" 

//De&ult Constructor 
quaternion: :quatenuonO 
{ 

qO = 1.0; 
ql = 0.0; 
q2 = 0.0; 
q3 = 0.0; 


quaternion: :quatemion(double angle_x, double angle_y, double angle_z, double rotation) 

{ 

//angle_x, angle_y, angle_z are the angles the axis of rotation make with the coordinate axes in radians 
//rotation is the amount of the rotation in radians 
qO = cosf(0.5*rotation); 
ql = cosf(angle_x)*sinf(0.5*rotation); 
q2 = cosf{angle_y)*sinf(0.5*rotation); 
q3 = cosf(angle_z)*sinfl[0.5*rotation); 


quaternion: :quatemion(const quatemion& q) 

{ 

qO = q.qO; 
ql=q.ql; 
q2 = q.q2; 
q3 = q.q3; 


void quaternion: :set(double angle_x, double angle_y, double angle_z, double rotation) 

{ 

//angle^x, angle_y, angle_z are the angles the axis of rotation make with the coordinate axes in radians 
//rotation is the amount of the rotation in radians 
qO = cosf(O.S*rotation); 
ql = cosf(angle_x)*sinf(O.S*rotation); 
q2 = cosf(aiigle_y)*sinf(0.5*rotation); 
q3 = cosf(angle_z)*sinf(0.5*rotation); 


} 

quatemion& quaternion: :(q)erator=(const quatemion& q) 

( 

qO = q.qO; 
ql=q.ql; 
q2*q.q2; 
q3 = q.q3; 
ietara*this; 

} 
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quaternion quaternion; ;operator+(const quatemion& q) 

{ 

quaternion sum; 
sum.qO = qO + q.qO; 
sum.ql “ql +q.ql; 
sum.q2 = q2 + q.q2; 
sum.qS q3 + q.q3; 
return sum; 


quaternion quatemion;:operator-(const quatemion& q) 

{ 

quaternion diff; 
diff.qO = qO - q.qO; 
diff.ql = ql - q.ql; 
difr.q2 = q2 - q.q2; 
diff.q3 = q3 - q.q3; 
return diff; 


quaternion quaternion; ;operator*(const quatemion& q) 

{ 

quaternion prod; 

prod.qO = (qO ♦ q.qO) - (ql * q.ql) - (q2 * q.q2) - (q3 * q.q3); 
prod.ql = (ql * q.qO) + (qO • q.ql) - (q3 * q.q2) + (q2 • q.q3); 
prod.q2 = (q2 * q.qO) + (q3 * q.ql) - (qO * q.q2) + (ql * q.q3); 
prodq3 = (q3 • q.qO) + (q2 * q.ql) - (ql ♦ q.q2) + (qO * q.q3); 
return prod; 


quaternion quaternion; ;operator*(doubie n) 

{ 

quaternion prod; 
prod.qO == qO * n; 
prod.ql * ql ♦ n; 
prod.q2 = q2 * n; 
prod.q3 = q3 * n; 
return prod; 

} 

double quaternion; ;magnitudeO 

{ 

return sqrt((qO • qO) + (ql • ql) + (q2 * q2) + (q3 ♦ q3)); 

} 

void quaternion; ;nomializeO 

{ 

double m * sqrt((qO * qO) + (ql * ql) + (q2 • q2) + (q3 • q3)); 
if(m) 

{ 

qO * ^ / m; 
ql = ql/m; 
q2 = q2 / m; 
q3 - q3 / m; 
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} 


quaternion quaternion; :rate_of_change(double P, double Q, double R) 

{ 

quaternion rate; 

rate.qO = -0.5*((ql * P) + (q2 * Q) + (q3 * R)); 
rate.ql = 0.5*((q0 ♦ P) + (q2 • R) - (q3 * Q)); 
rate.q2 = 0.5*((q0 • Q) + (q3 * P) - (ql * R)); 
rate.q3 * 0.5*((q0 * R) + (ql * Q) - (q2 • P)); 
return rate; 


void quaternion; ;update(double P, double Q, double R, double sec) 

{ 

//Runga Kutta fourth order method used 
double hh = sec * .5, h6 = sec / 6; 
quaternion y = •this, dym, dyt, yt, dydx; 
dydx qO = -0.5*((ql * P) + (q2 • (J) + (q3 • R)); 
dydx.ql « 0.5*((q0 • P) + (q2 * R) - (q3 • Q)); 
dydx.q2 = 0.5*((q0 • Q) + (q3 • P) - (ql * R)); 
dydx.q3 = 0.5*((q0 • R) + (ql • Q) - (q2 • P)); 

yt.qO = y.qO + hh ♦ dydx.qO; 
ytql = y.ql + hh ♦ <fydx.ql; 
yt.q2 = y.q2 + hh • ^dx.q2; 
yt.q3 = y.q3 + hh • <fydx.q3; 

dyt.qO = -0.5*((yt.ql • P) + (yt.q2 * Q) + (yt.q3 * R)); 
dyt ql = 0.5*((yt.q0 • P) + (yt.q2 • R) - (yt.q3 ♦ C»); 
dyt.q2 = 0.5*((yt.q0 * C» + (yt.q3 • P) - (yt.ql ♦ R)); 
dyt.q3 = 0.5*((ytq0 • R) + (yt.ql • Q) - (ytq2 ♦ P)); 

yt.qO = y.qO + hh • dyt.qO; 
yt.ql = y.ql + hh • dyt.ql; 
yLq2 = y.q2 + hh * (^.q2; 
ytq3 = y.q3 + hh • ^.q3; 

dym.qO = -0.5*((yt.ql * P) + (yLq2 • Q) + (yt.q3 * R)); 
<^.ql = 0.5*((ytq0 • P) + (ytq2 • R) - (yLq3 ♦ (J)); 

<^.q2 = o.5*((yt.^ • CD + (yt-q3 * P) - (yt.ql * R)); 
dym.q3 = 0.5*((yt.q0 • R) + (yt.ql * Q) - (yt.q2 * P)); 

ytqO = y.qO + sec • dyni.qO; 
yt.ql = y.ql + sec • (!^.qr, 
ytq2 = y.q2 + sec * d^.q2; 
yt.q3 =* y.q3 + sec • (^.q3; 

c^.qO - (fym.qO + dyt-qO; 

<^.ql = c^.ql + ^.ql; 

(^.q2 “ (^.q2 + (iGt.q2; 

(fyin.q3 * <^.q3 + (fyt.q3; 
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dyt.qO = -0.5*((yt.ql * P) + (yt.q2 * Q) + (yt.q3 * R)); 
(fyt.ql = 0.5*((yt.q0 • P) + (yt.q2 * R) - (yt.q3 * Q)); 
dyt.q2 = 0.5*((yt.q0 * Q) + (yt.q3 * P) - (yt ql ♦ R)); 
dyt.q3 = 0.5*((yt.q0 * R) + (yt ql ♦ Q) - (yt.q2 * P)); 

qO = y.qO + h6 * (dydx.qO + dyt.qO + 2.0 * dym.qO); 
ql = y.ql + h6 * (dydx.ql + dyt.ql + 2.0 • dym.ql); 
q2 = y.q2 + h6 * (^dx.q2 + dyt.q2 + 2.0 * dyin.q2); 
q3 = y.q3 + h6 * (dydx.q3 + dyt.q3 + 2.0 * <^Tn.q3); 


} 

quaternion quaternion: ;rate_of_change(vector3D ang_velocit}’) 

{ 

quaternion rate; 

rate.qO = ■0.5*((ql * ang_velocity(OJ) + (q2 * ang_velocityIll) + 
(q3 ♦ ang_velocily[21)); 

rate.ql = 0.5*((q0 * ang_velocityI01) + (q2 * ang_veIocityl21) - 
(q3 * ang_veiocity[lJ)); 

rate.q2 = O.S*((qO * ang^velocity[l]) + (q3 * ang_velocityIO]) - 
(ql * ang_velocity[21)); 

rate.q3 = 0.5*((q0 * ang_velocily[2]) + (ql * ang_velocityIl]) • 
(q2 ♦ ang_velocity(01)); 

return rate; 


void quaternion: :iqxlate(vcctor3D ang_veloci(y, double sec) 

{ 

//Runga Kutta method used 

double P = ang_vclocity[0], Q = ang_velocity[l], R = ang_velocityI2]; 
double hh = sec ♦ .5, h6 = sec / 6; 
quaternion y = ♦this, dym, dyt, yt, dydx; 

dydx.qO = -0.5*((ql • P) + (q2 ♦ Q) + (q3 * R)); 
dydx.ql = 0.5*((q0 * P) + (q2 ♦ R) - (q3 * Q)); 
dydx.q2 = 0.5*((q0 * Q) + (q3 * P) - (ql * R)); 

<fydx.q3 = 0.5*((q0 * R) + (ql * Q) - (q2 ♦ P)); 

yt.qO = y.qO + hh * dydx.qO; 
yt.ql - y.ql + hh * <fydx.ql; 
yt.q2 = y.q2 + hh * dydx.q2; 
yt.q3 = y.q3 + hh * €tydx.q3; 

<fyt.qO = -0.5*((yt.ql ♦ P) + (yt.q2 * Q) + (ytq3 ♦ R)); 
dytql = 0.5*((ytq0 • P) + (yt.q2 • R) - (yt.q3 ♦ Q)); 
dyt.q2 = 0.5*((yLq0 * (J) + (yt.q3 ♦ P) - (ytql * R)); 
dytq3 = 0.5*((yt.q0 * R) + (ytql * Q) - (ytq2 ♦ P)); 


yt.qO = y.qO + hh * <fytqO; 
ytql = y.ql + hh * ^.ql; 
ytq2 = y.q2 + hh • dyt.q2; 
ytq3 “ y.q3 + hh * ^.q3; 
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dym-qO - 4).5'‘((yt.ql * P) + (yt.q2 * Q) + (yt.q3 * R)); 
dym.ql = 0.5*((yt.q0 * P) + (yt.q2 * R) - (yt.q3 * Q)); 
dyin.q2 = 0.5*((yt.q0 * Q) + (yt.q3 • P) - (ytql * R)); 
dym qS = 0.5*((yt.q0 * R) + (yt ql * Q) - (yt.q2 • P)); 

ytqO = y.qO + sec • dyin.qO; 
ytql = y.ql + sec ♦ c^.ql; 
yt.q2 = y.q2 + sec * <^.q2; 
yt.q3 = y.q3 + sec * <^.q3; 

dym.qO = dyin.qO + dyt.qO; 

^rin.ql = (^nDa.ql + (^.ql; 

^an.q2 = (fyin.q2 + (^.q2; 

^in.q3 = (fym.q3 + clyt.q3; 

dyt.qO = -0.5*((yt.ql * P) + (yt.q2 * Q) + (yt.q3 * R)); 
dytql * 0.3*((yt.q0 * P) + (yt.q2 ♦ R) - (yt.q3 * Q)); 
dyt.q2 = 0.5*((yt.q0 • Q) + (yt.q3 * P) - (yt.ql * R)); 
dyLq3 = 0.5*((yt.q0 * R) + (yt.ql * Q) - (yt.q2 * P)); 

qO = y.qO + h6 * (dydx.qO + dyt.qO + 2.0 * dyin.qO); 
ql = y.ql + h6 * (dydx.ql + dyt.ql + 2.0 * dyni.ql); 
q2 = y.q2 + h6 * (^dx.q2 + <^.q2 + 2.0 * (^.q2); 
q3 =y.q3 + h6 • (dydx.q3 + (^.q3 + 2.0 • d^.q3); 

} 

ostream& operator«(ostreain& os, quatemion& q) 

{ 

os « (double) q.qO « ","« (double) q.ql « ", " « (double) q.q2 «", " «(double) q.q3 « 
"\n"; 

return os; 

} 

double& quatenuon::operatorQ(int n) 

{ 

if(n = 0) 

{ 

return qO; 

} 

if(n=l) 

{ 

return ql; 

} 

if(n = 2) 

{ 

return q2; 

} 

if(n = 3) 

{ 

return q3; 

} 

> 

#eiidif 
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APPENDIX G 


Matrix3x3 Code 

A. HEADER FILE 

#ifedefMATRIX3X3_H 
#define MATRIX3X3_H 
#mclude "vectorSD.H" 

#include "quaternion.H" 

^include <iostream.h> 

class iiiatrix3x3 

{ 

double m[9]; 
public; 

iiiatrix3x30; 

inatrix3x3(double, double, double, double, double, double, double, double, double); 

inatiix3x3(const matrix3x3&); 

tiiatrix3x3& operator>(coiist iiiatrix3x3&); 

matrix3x3 operator+(const iiiatrix3x3&); 

inatrix3x3 operator*(coiist matrix3x3&); 

iiiatrix3x3 operator*(const iiiatrix3x3&); 

void DCM_x_iotatioa(double); 

void DCM_y_iotatioii(double); 

void DCM_z_rotation(double); 

void DCMJbody_to_world(quatemion); 

void DCM_world_to_body(quateniion), 

void tranqx>seO; 

quatemioQ geaerate_orieQtationO; 
vector3D opcrator*(vector3D&); 
iiiatrix3x3 operator*(double); 
double& operatorQ(int); 

friend ostreain& operator«(ostreani&, niatrix3x3&); 

~matrix3x30 { } 

}; 

#endif 
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B. SOURCE FILE 
#ifiidefMATRIX3X3_C 
#define MATRIX3X3_C 
#incliide ”iiiathx3x3.H'' 


inatiix3x3: :matiix3x30 

{ 

m(0] = 1; 
n»[l] = 0; 
mil] = 0; 
in(3] = 0; 
in(41 = 1; 
mlS] *= 0; 
in[6] = 0; 
in(7J = 0; 
in[81 = 1; 


} 

inatrix3x3::iiiatiix3x3(double a, double b, double c, double d, double e, double f, double g, double h, 
double i) 

{ 

m[0] = a; 
m[ll = b; 
n»(21 = c; 
in[31 = d; 
in[4i = e; 
in[51=f; 
ni[6i = g; 
ni[7] = h; 
in[81 = i; 

} 

inatrix3x3::inatrix3x3(const inatrix3x3& v) 

{ 

in[0] = v.ni[0]; 
inll] = v.inil]; 
in[2] = v.in(21; 

10(3] = v.in[3]; 
in[4J = v.in(4]; 
m[5i -v.mis]; 
in[6] = v.in[6]; 
in[7] “v-miT]; 
misj =v.in[8]; 

} 

inatrix3x3& inatrix3x3::operatorKcoiist ii]atrix3x3& v) 

{ 

mfOJ = v.ni[0]; 

in{2] ■ v.id[2]; 

mi3i“v.iiiI31; 
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ni(4] = v.m[4] 

mis] = v.mlsi 

ini6] = v.ni[6] 
ml?] = v.mi?] 
in[81 “ v.in[8] 
return *this; 


inatrix3x3 inatrix3x3::operator+(const n>atrix3x3& v) 

{ 

matrix3x3 sum; 
sum.m[0] = m[0] + v.m(0]; 
sum.m|li - mil] + v.m[lj; 
sum.m|2j = m[2] + v.mi2]; 
sum.m[3i = m|3] + v.mi3J; 
sum.m[4] = mi41 + v.m[4i; 
sum.m[5] = m[5J + v.m[5J; 
sum.m[6] = m[6] + v.mi6]; 
sum.ml?] = m[71 + v.mi?]; 
aun-mis] = m[8] + v.mi8]; 
return sum; 

} 

matrix3x3 matrix3x3::operator-(const matrix3x3& v) 

{ 

matrix3x3 difference; 
difference.m(0] = mfOJ - v.mfOJ; 
difference.m|lj = mil] -v.mil]; 
difference.mi2j = m^j - v.m[2]; 
diffetence.m[3] = mp] - v.mpj; 
dififetence.mi4] = m[41 - v.mpj; 
difference.mis] = mp] - v.mis]; 
difference.mp] = mpj - v.m[6]; 
difference.m[7] = mpj - v.mpj; 
di£ference.m[8] = mi8] - v.m[8]; 
return difference; 

} 

//matrix midtiplication 

matrix3x3 matiix3x3::operator*(const matrix3x3& v) 

{ 

matrix3x3 temp; 

temp.m[0] = (m[0] * v.m[0]) + (m[l] * v.m(3]) + (m[2] * v.m[6]) 
temp.mil] = (mio] * v.milj) + (milj * v.mpj) + (m[2] * v.miT]) 
temp.mi2] “ (mioj * v.mi2]) + (mil] * v.mis]) + (mi2) • v.mi8]) 
temp.mi3] = (mi3J * v.mio]) + (m[4i * v.mi3j) + (mp] * v.mpj) 
tenq>.mi4] ** (mi3] ♦ v.milj) + (mpj * v.mpj) + (mpj * v.mpj) 
temp.misj = (mi3i * v.mpj) + (mpj * v.mjsj) + (mjsj * v.mi8j) 
tBiiq).mi6j = (mjdj * v.mjoj) + (mjT] * v.mi3j) + (mjsj • v.mi6j) 
temp.mi7j =• (mpj * v.milj) + (mpj ♦ v.mpj) + (mpj * v.m{7j) 
temp.mi8j = (mpj * v.mi2j) + (mpj * v-misj) + (mpj * v.mi8j) 
return temp; 


} 
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//matrix multiplication with a vector 
vector3D matrix3x3:;opcrator*(vector3D& v) 

{ 

vector3D temp - v; 

templO] = (m[01 • vlO]) + (m[ll * vllj) + (mpj * v[21); 
tempfll = (mI3J • vfO]) + (m(4] • v{lj) + (m(5J * vp}); 
tempjlj = (mlej * vlO]) + (mj?] * vllj) + (m[8i ♦ v[2i); 
return temp; 


//scalar multiplication 

matrix3x3 matrix3x3::operator*(double n) 

{ 

matrix3x3 product; 
productmfO] = m[0] * n; 
product.m|li * mil] ♦ n; 
product.m|2] = mi2i • n; 
product.m[3] > m|3j * n; 
product.m[4] - mi41 * n; 
product.m[sj = miS] ♦ n; 
product.m{6] = m[6] * n; 
product.mi7] = m[7] * n; 
product.m[8] = mi8i ♦ n; 
return product; 

} 

ostreain& operator«(ostream& os, matrix3x3& v) 

{ 

os «(double) v.m[0] « ","« (double) v.m(l]« ", " «(double) v.m[2]«"\n" 

« (double) v.m[3]«"," «(double) v.ml4]« ", " «(double) v.m[5] «"\n" 

« (double) v.m|6J« ", " « (double) v.m|7j« ", " « (double) v.m|8J« "\n"«"\n"; 
return os; 

> 

double& matrix3x3;:operatorD(int y) 

{ 

return m(y]; 

} 

//genmates DCM for rotation about the x axis 
void matrix3x3::DCM x_rotation(double angle) 

{ 

mlO] = 1.0; 
mil] = 0.0; 
m[2] = 0.0; 
m[3]=0.0; 
m[4] = cos(angIe); 
m|S] = sin(angle); 
m{6] = 0.0; 
m|7] = -sin(angle); 
mjsj = cos(angle); 
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//generates DCM for rotation about the y axis 
voidniatrix3x3;;DCM_y rotation(double angle) 

{ 

ni(0] = cos(angle); 
mill = 0.0; 
mil] = -sin(angle); 
ml3] = 0.0; 
ml4] = 1.0; 
mI5] = 0.0; 
m|6] = sin(angle); 
mI7] = 0.0; 
m[8] = cos(angle); 

} 

//generates DCM for rotation about the z axis 
void niatrix3x3::DCM_z_rotation(double angle) 

{ 

m[0] = cos(angle); 
mjlj = sin(angle); 
ml2] = 0.0; 
mPl = -sin(angle); 
m|4j = cos(angle); 
m[5] = 0.0; 
mI6] = 0.0; 
mI7]=0.0; 
ml8] = 1.0; 

} 

//generates DCM for transformation from body to world coordinates 
voidmatrix3x3::DCM bo^ to world(quatemion orientation) 

{ 

mlO] = ((orientationfO] * orientation[0]) + (orientation[l] * 
orientation[l]) - (orientation[2] * orientation[2]) - 
(orientation(3] * orientation[3])); 
m[l] = 2.0 * ((orientation! 1) * orientation[2]) - (orientation[0] * 
orientation[3])); 

ml2] = 2.0 * ((orientation(0] * orientation[2]) + (orientation! 1] ♦ 
orientation!3])); 

m!3] = 2.0 • ((orientation! 1] * orientation!2]) + (orientationlO] * 
orientation!3])); 

m!4] = ((orientationlO] * orientationlO]) - (orientation] 1] * 
orientation]!]) + (orientation!2] * orientation!2]) • 
(orientationI3] * orientation!3])); 
mlS] = 2.0 * ((orientation!2] * orientation!3]) • (orientationlO] * 
orientationll])); 

ml6] = 2.0 * ((orientationll] * orientation^]) - (orientationlO] * 
orientationl2])); 

ml7] = 2.0 * ((orientationl2] * orientationl3]) + (orientationlO] * 
orientationll])); 

ml8] = ((orientationlO] * orientationlO]) - (orientationll] * 
orientationll]) - (orientationl2] * orientation]^]) 4- 
(orientation^] * orientationl3])); 

} 
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//gencfates DCM for transformation from world to body coordinates 
voidmatrix3x3::DCM world to body(quatemion orientation) 

{ 

m[0J = ((orientationfO] • orientationfO]) + (orientation! 1] • 
orientation[l]) • (orientation[2] * orientation[2]) - 
(orientation[3] * orientaticn[31)); 
m[3| = 2.0 * ((orientation! 1] • orientation!2]) - (orientation!0| * 
orientation!3])); 

m!6] = 2.0 * ((orientation!0] ♦ orientation!2]) + (orientation! 1] • 
orientation!3])); 

m!l] = 2.0 • ((orientation!!] * orientation(2]) + (orientation!0] * 
orientation!3])); 

m(4)» ((orientation!01 * orientation!0]) • (orientation! 1] * 
orientation] 1]) + (orientation!21 * orientation!2]) - 
(orientation!3] * orientation!3])); 
m]?] = 2.0 * ((orientation!2] * orientation!3]) - (orientation!0] ♦ 
orientation] 1])); 

m!2] = 2.0 * ((orientation] 1] ♦ orientation!3]) - (orientation]©] * 
orientation!2])); 

m(5] = 2.0 • ((orientation!21 ♦ orientation(3]) + (orientation!0] • 
orientation] 1])); 

m[8] = ((orientation]©] ♦ orientation]©]) - (orientation]!] * 
orientation]!]) • (orientation]2] * orientation]2]) + 
(orientation]3] * orientation]3])); 

} 

void matrix3x3::tiansposeO 

{ 

matrix3x3 v = •this; 
m]0J = v.m(0]; 
m]!] = v.m]3]; 
m]21 = v.m]6]; 
m]3] = v.m]!]; 
ml4] = v,ml4]; 
m]5]=v.m]7J; 
ml6] = v.m]2]; 
m]7] =v.m]5]; 
m]8] = v.ml8]; 

} 

quaternion mattix3x3;:generate orientationQ 

{ 

quaternion q; 

q]0] = 0.5 • sqrt(l + m]0] + m]4] + m]81); 
q]!] = 0.5 * sqrt(! + m]0] - m]4] - m]8]); 
q]21 = 0.5 * sqrtd - m]©] + m]4J - m]8]); 
q]31« 0.5 • sqrt(l - m]0] - m(41 + m]8]); 
q.normalizeO; 
rtiumq; 

} 

«endif 
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APPENDIX H 


Time Code 


A. HEADER FILE 
#ifhdefTIME_H 
#define TIME_H 
#include <time.h> 
#include <sys/types.h> 
^include <sys/tiincs.h> 
^include <^s/parain.h> 


void set_deltaO; 
void set_delta(double); 
void set_tiineO'. 
void reset_timeO; 
double read_deltaO; 
double read_timeO; 
int read_ticksO; 

void set_rcal_time_factor(double); 
#endif 
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B. SOURCE FILE 
#ifiidef TIME_C 
#define TIME_C 
^include "time.H" 

struct tins timebuffer; 
long old_time; 

double delta, real_time_ratio = 1.0; 

void set deltaO 

{ 

// HZ is the ^stem clock rate in hertz 

delta = {(double) (tiines(&timd)u£fer) - old_time)/HZ) * real_time_ratio 
old time tiines(&timebufrer); 

} 

void set deita(double step) 

{ 

delta - step; 

old time == timesC&timebuffer); 

} 

void set timeO 

{ 

old time = times(&timebuffer); 

} 

double read deltaQ 

{ 

return delta; 

} 

double read timeQ 

{ 

return (double) old_time; 

} 

intread ticksQ 

{ 

return (int) times(&timebuffer); 

} 

void set real time &ctor(double 1) 

{ 

real_time_ratio = f; 

> 

void resetJimeO 

{ 

l<mg delta_tidcs; 

delta_ticks = (long) (delta * HZ); 

old time »times(&timdnifier) - delta_ticks; 

> 

#eiidif 
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APPENDIX I 


Graphics Code 


A. HEADER FILE 
#ifiadcfGRAPHICS_H 
#defincGRAPHICS_H 
#include <math.h> 

^include "vectorSD.H" 

^include "inatrix3x3.H" 

#include "quaternion.H" 

#include "itlobj_opcodes.h" 

#include "rdobj_fiincs.h" 

^include <stdio.h> 

#mclude <gl.h> 

#mclude <device.h> 

//initializes the graphic system 
void initializeO; 

//initializes control window 
void init_control_windowO; 

//make viewing window active 
void main_windowO; 

// makes control window active 
void control_windowO; 

//clears control window 
void clear_control_window0; 

//control window for euler program 

void culer_controls(int, int, int, int, int, int, int,quatemion, double); 

//control window for gyro program 

void gyio_controls(int, int, int, int, vector3D, int, int, int, double, double, double, double); 
//statisic display for gyro program 

void stat_controls(double, double, double, double, double, double, double, double, vector3D*); 
//control window for frame program 

void frame_controls(int, int, vector3D, vectorSD, vector3D, int); 

//standard function for viewing a scene 
voidviewO; 

//used to view the scene for a point of view fixed to a rigid body 
void viewfquatemion, vectorSD, int); 
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//attaches the to a rigid body 
void attach jeye_to(vector3D*, iot*); 

//attaches the tatget to a rigid body 
void attach_targct_to(vector3D*); 

//attaches the tyt to a rigid botfy 
void set_eyc_to(double, double, double); 

//attaches the target to a rigid body 
void set_target_to(double, double, double); 

//rotates the view in tenths of degrees 
void rotate_view(int); 

//displays the body axes of a rigid_body 
void view_axisO; 

//gravity check - returns non zero value when gravity is turned on 

int gravity_statusO; 

void set_^vity_onO; 

void set_gravity_oflft); 

void toggle,jravityO; 

//air resistance check • returns non zero value when air resistance is turned on 

int air_resistance_statusO; 

void set_air_resistancc_onO; 

void sct_air_iesistancc_oflRO; 

void toggle_air_resistanceO; 

// c routines the must be accessed 
extern "C" 

{ 

extern OBJECT* read_()bject(char[]); 

extern void ready_object_for_display( OBJECT* ); 

extern void display this object( OBJECT* ); 

}; 

#endif 
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B. SOURCE FILE 
#ifiidefGRAPHlCS_C 
#define GRAPHICS_C 
#indude "graphics.H" 

#define NEARDEPTH 0x000000 /* the near and far planes used for Zbuffering*/ 
#define FARDEPTH 0x7fiBBBf 

OBJECT *lightobj, ♦axis; 

//eye and target are the global variables that control the view point 
//and reference point of the scene respectively 

vector3D *cye = new vector3D(10.0,10.0,10.0), *target = new vector3D; 

int *Qfe_display_field = NULL; 

int gravity_flag ® 0, air_resistance_flag = 0; 

int twist = 0; 

long main_win, control_win; 

Matrix un = { 1.0,0.0,0.0, 0.0, 

0 . 0 , 1 . 0 , 0 . 0 , 0 . 0 , 

0.0,0.0,1.0,0.0, 

0.0,0.0,0.0, 1.0}; 

int gravity statusQ 

{ 

return gravity flag; 

} 

void set_gravily_on0 

{ 

gravity flag= 1; 

} 

void set jgravity ofiPO 

{ 

gravity flag = 0; 

} 

void toggle_jravity0 

{ 

if(gravity_flag) 

{ 

gravity flag = 0; 

} 

else 

{ 

gravity flag = 1; 

} 

} 

int air resistance statusQ 

{ 

return air_iesistance flag; 

> 
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v(Mdset_air resistance_onO 

{ 

air resistance_flag => 1; 

} 

void set air resistance_ofiO 

{ 

air resistance_flag = 0; 

} 

void toggle air_resistaiiceO 

{ 

il(air resistance flag) 

{ 

air_resistance flag^O; 

> 

else 

{ 

air resistance_flag = 1; 

} 


void initializeO 

{ 

/* set up the preferred aspect ratio */ 
kecpaspect(XMAXSCREEN+l,YMAXSCREEN+l); 
pre&ize(XMAXSCREEN/2,YMAXSCREEN/2); 
pre^tion(O.XMAXSCREEN * 0.8.0.YMAXSCREEN • 0.8); 
I* open a window for the program */ 
mainjwin “ win<q)en("M^"); 

wintitle("Physically Based Reality, A Keith Haynes Production"); 

/* put the nUS into double buffer mode *! 

doublebufferO; 

/* put the iris into rgb mode *! 

RGBmodeQ; 

t* configure the IRIS (means use the above command settings) *! 
gconfigO; 

/* s^ the depth for z-buffering *! 
lsetdepth(NEARDEPTH,FARDEPTH); 

/* queue the redraw device */ 
qdeviceCREDRAW); 

/* queue the memdnitton */ 
qdevice^flENUBUTTON); 

I* turn die cursor on *! 
cursonQ; 

/* select gouraud shading */ 
shaderttodelCGOURAllD); 

/* turn on the rww projection matrix mode */ 
tnmode(MVIEWING); 

/*Tum of Zbuffering*/ 
dmffintTRUE); 

lightobj » read_object(*the_lighLoff"); 
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axis “ itad_object("frame.oflr‘); 

ieady_object_for_display(lightobj); 

rea<fy_object_for_display(axis); 

//queue up input devices 

qdsvice(LEFTMOUSE); 

qdevice(RIGHTMOUSE); 

qdevice(MOUSEX); 

qdevice(MOUSEY); 

qdeviceOUGHTARROWKEY); 

q^ce(LEFTARROWKEY); 

qdevice(UPARROWKEY); 

qdcvicc(DOWNARROWKEY); 

qdevice(SPACEKEY); 

qdevice(EQUALKEY); 

qdevicc(MINUSKEY); 

qdevice^lKEY); 

qdevice(F2KEY); 

qdevice^3KEY); 

qdevice(F4KEY); 

qdevice(F5KEY); 

qdevice(F6KEY); 

qdcvice(F7KEY); 

qdevice(F8KEY); 

qdevice(F9KEY); 

qdevice(F10KEY); 

qdevice(Fl IKEY); 

qdevice(F12KEY); 

HcXtax draw and display buffer 

czclear(0xITFF7200,getgdcsc(GD_ZMAX)); 

sw^uffersO; 

czclear(0xFFFF7200,getgdesc(GD ZMAX)); 

} 

void init_control_windowO 

{ 

/* set up the preferred aspect ratio *! 

prefposition(O.XMA> SCREEN * O.S.YMAXSCREEN * 0.87, YMAXSCREEN); 
/* (q)en a window for the rrogram */ 
controI_win == winopeuCcontrol"); 
wintitl^"System Control Window"); 

I* put the IRIS into double buffer mode *! 
doubldnifferO; 

RGBmodeO; 

gconfigO; 

pushmatrixO; 

oitho2(0.0,769.0,0.0,100.0); 

RGBcolor(255,255,255); 

clearO; 

popmatrixO; 

svt^uffersO; 

} 

void mainjvindowO 

{ 
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winset(main_wm); 


} 

void control windowO 

{ 

winset(control win); 

} 

void clear control windowQ 

winset(control_win); 

pusiunatrixO; 

ortho2(0.0, 769 0 , 0.0,100.0); 

RGBcolor(255.255,255); 

dearO; 

popmatrixO; 

swyibiififersO; 

winset(inain_win); 

> 

void euler_conttols(int angl, int ang2, int ang3, int axisl, int axis2, int axis3, int q, quaternion 
orientation, double theta) 

{ 

//angl, aiig2, ang3 are the amounts of the rotations 

//axisl, axis2, axis3 are the about which the shuttle is rotated 

//q is t^ flag for placing an X in the quaternion equivilant box 

//orientation is the quaternion from the shuttle 

//theta is the value for the rotation using a single quaternion rotation 

winset(control_win); 

//up and down arrows 
float ptl31[2] = { 

{202,58}, 

{208,58}, 

{205,52}}; 
float pt2[31[2] = { 

{142,52}, 

{148,52}, 

{145,58}}; 
char s[32]; 
pushmatrixO; 

ottho2(0.0.769.0,0.0,100.0); 

RGBcolor(255,255,255); 

clearO; 

IIsA axis color for rotation 1 
switch(axisl) 

{ 

case 1: 

RGBcolor(200,0,0); 

break; 

case 2: 

RC»color(0,0,200); 

l»eak; 
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case 3; 

RGBcoMO.O.O); 

break; 


default; 

RGBcolor(200,0.0); 

break; 

} 

//draw boxes for first rotation angle and axis 
rectf(150.0, 50.0,200.0,60.0); 
rectf(170.0. 35.0,180.0,45.0); 

//set axis color for rotation 2 
switch(axis2) 

{ 

case 1: 

RGBcolor(200,0,0); 

break; 

case 2: 

RGBcolor(0,0,200); 

break; 

case 3; 

RGBcolor(0,0,0); 

break; 

default; 

RGBcolor(0,0,200); 

break; 

} 

//draw boxes for second rotation angle and axis 
iectf(250.0,50.0,300.0,60.0); 
rectf(270.0,35.0,280.0,45.0); 

//set axis color for rotation 3 
switch(axis3) 

{ 

case 1: 

RGBcolor(200,0,0); 

break; 

case 2: 

RGBcolor(0,0,200); 

break; 

case 3: 

RGBcolor(0,0,0); 

break; 

defoult: 

RGBcolor(0,0,0); 

break; 






} 

//draw boxes for third rotation angle and axis 
iectf(350.0.50.0,400.0,60.0); 
rectf(370.0. 35.0, 380.0,45.0); 

RGBcoIor(200,200,200); 

//area for up and down arrows 
rectf(140.0,50.0,150.0,60.0); 
rectf(200.0,50.0,210.0,60.0); 
iectf(240.0, 50.0,250.0,60.0); 
rectf(300.0.50.0,310.0,60.0); 
rectf(340.0, 50.0, 350.0,60.0); 
rcctf(400.0, 50.0,410.0,60.0); 

//reset and go buttons 
rectf(550.0,25.0,600.0,75.0); 
iectf(625.0,25.0,675.0,75.0); 
RGBcolor(0,0,0); 

// Draw up and down arrows 

polC(3,pt); 

polC(3.pt2); 

ptI0]f0]=pt[01[0J +100.0; 
pt[l](01 = ptllJl0J +100.0; 
pt[2][0J“pt[2H0J +100.0; 

pt2[0][0]»pt2{0][0] +100.0; 

pt2[ll[0J = pt2(l](0I +100.0; 
pt2[2J(0] = pt2I2][01 + 100.0; 
polf2(3,pt); 
pol£2(3,pt2); 

pt[01(01 = pt(01[01 +100.0; 
pt{ll(01=»pt(ll(0j +100.0; 
ptl2][0]»ptt2I[0] +100.0; 
pt2[01[01 = pt2[0H01 + 100.0; 
pt2{lII0] = pt2[lH0} +100.0; 
pt2[21[0J = pt2[2J(0] + 100.0; 
polf2(3,pt); 
pol£Z(3,pt2); 

//Show Quaternion; 

RGBcolor(0,0,255); 
iectf(190.0,15.0,200.0,25.0); 
RGBcolor(255,255,0); 

m 

{ 

cinov2(193.5,17.0); 

chaistr("X"); 

} 

//Rotation Ou^ut 
RGBcolor(255,0,0); 
rectfi[420.0,10.0,520.0,80.0); 
RGBcolor(0,0,0), 
cinov2(425.0,66.0); 

diarsti<Tl»eta =•); 
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cinov2(42S.0,S1.0); 
charstr("qO ="); 
cmov2(42S .0,41.0); 
cliarstr("ql 
cniov2(425.0,31.0); 
charstr("q2 ="); 
cinov2(425.0,21.0); 
charstr("q3 ="); 
RGBcolor(255.255.255); 
cmov2(475,66); 

sprintfts, "%.ir. theta ♦ 57.29578); 

cbaTstr(s); 

cmov2(455, 51); 

q)rintf(s, "%.4r. orientation[0]); 

charstr(s); 

cmov2(455,41); 

sprintf(s, "%.4r, orientation! 1]); 

charstr(s); 

cmov2(455,31); 

q>rintf(s, "%.4r, orientation[2]); 

cliarstr(s); 

cniov2(455,21); 

9rintfi(s, "%.4r, orientation(3]); 
charstr(s); 


//Control Window text 

RGBcolor(0,0,0); 

cinov2(10.0,90.0); 

cliarstr("Euler Rotation Control Window*); 
cmov2( 10,50); 

cliarstr("Rotation in Degrees"); 
cinov2(10,35); 
charstr("Axis of Rotation"); 
cniov2(10,15); 

cliarstr("Show Quaternion Equivalent"); 

cmov2(570,45); 

charstr("GO"); 

cmov2(635,45); 

charstr(”Reset"); 


//angles and axis output 
RGBcolor(255.255,0); 
cinov2(160,51); 

sprintfi(s, "%.or, (double) angl); 

charstr(s); 

cniov2(260, 51); 

qnintfCs, ”%.0r, (double) ang2); 

cliarstr(s); 

cmov2(360, 51); 

q;>rint£[s, *%.0r, (double) aiig3); 

charstr(s); 

cinov2(172,36); 





qtfintfits, *%.or. (double) axisl); 

diarstrCs); 

cinov2(272, 36); 

9 rintf(s, ”%.or, (double) axis2); 

chafstT(s); 

cinov2(372.36); 

8priiitf(s. ”%.or, (double) axis3); 
charstr(s); 

popmatrixO; 

swqibufrersO; 

} 


void &aine_coiitrols(int vlevel, int ilevel, vectorSD mag, vector3D pos, vector3D av, int vaxis) 

{ 

/A'level - viewing level, flevel • assignment level, mag - linear velocity, pos • position 
//av angular velocity, vaxis - viewing axis 
float ptI31I21-{ 

{192,48}. 

{198,48}, 

{195,42}}; 
float pt2{31{2]»{ 

{232,42}, 

{238,42}, 

{235,48}}; 
winset(control_win); 
char s(32]; 
pushmatrixO; 

ortho2(0.0,769.0,0.0,100.0); 

RGBcolor(255,255,255); 

dearO; 

//Go ft Reset Buttons 
RGBcolor(200,200,200); 
rectfl:550.0,25.0,600.0, 75.0); 
rectf(625.0,25.0,675.0,75.0); 

RGBcolor(0,0,0); 
cniov2(570,45); 
charstr("GO"); 
cinov2(635,4S); 
charstrCllesetS; 

// Select viewing level 
RGBoolor(0.0,25S); 
lectflClO.0,0.0,65.0,60.0); 

RGBcolor(25S,25S,0); 
cmov2(l 1.0,51.5); 
charstrCInertial"); 
cinov2(35.0,41.5); 
charstrCl"); 

Ciiiov2(35.0.31.5); 

cliarstrC2"); 

cin0v2(35.O,21.5), 
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charstr(“3"); 

ciiiov2(35.0,11.5); 

charstr("4"); 

ciih)v2(35.0,1.5); 

charstr(*5"); 

switch(vlevel) 

{ 

caseO; 

rectfKlO.O. 50.0,65.0,60.0); 
RGBcolor(0.0,255); 
cmov2(l 1.0,51.5); 
charstrCInertial"); 
break; 

case 1: 

iectf(10.0,40.0,65.0, 50.0); 

RGBcolor{0,0.255); 

cmov2(35.0,41.5); 

charstr("r); 

break; 

case 2: 

rectf(10.0, 30.0,65.0,40.0); 

RGBcolor(0,0,255); 

cmov2(35.0,31.5); 

charstr("2"); 

break; 

case 3: 

rectHlO.O, 20.0,65.0, 30.0); 

RGBcolor(0,0,255); 

cmov2(35.0,21.5); 

charstr("3"); 

break; 

case 4: 

rcctf(10.0.10.0,65.0,20.0); 

RGBcolor(0,0,255); 

cinov2(35.0,11.5); 

charstr("4"); 

break; 

case 5: 

rectf(10.0,0.0,65.0,10.0); 

RGBcolor(0,0,255); 

cmov2(35.0,1.5); 

charstr("5"); 

break; 

default: 

iectf(10.0,50.0,65.0,60.0); 
RGBcolor(0,0,255); 
ciiiov2(l 1.0,51.5); 


diaistr("Iiieitial"); 

break; 


// Select fiame assignment level 

RGBcolor(0,0,255); 

rectf(70.0.0.0,110.0, 50.0); 

RGBcolor(255.255.0); 

cmov2(88.0.41.5); 

chaistr(T); 

cmov2(88.0,31.S); 

chaisti("2'‘); 

Cinov2(88.0.21.5); 

cliaistr("3"); 

cmov2(88.0,ll.S); 

cliarstr("4"); 

cmov2(88.0,1.5); 

charstr("5*); 

switcli(flevel) 

{ 

case 1: 

rectf(70.0,40.0,110.0, 50.0); 

RGBcolor(0,0,255); 

cmov2(88.0,41.5); 

chaistrCD; 

break; 

case 2; 

iectfl[70.0, 30.0, 110.0, 40.0); 

RGBcolor(0,0,255); 

cniov2(88.0,' 1.5); 

cliarsti("2"); 

break; 

case 3: 

tectf(70.0,20.0,110.0,30.0); 

RGBcolor(0,0,255); 

cinov2(88.0,21.5); 

charstr(-3"); 

break; 

case 4; 

iectfl(70.0,10.0,110.0,20.0); 

RGBcolor(0,0,255); 

cinov2(88.0,11.5); 

diaretr("4"); 

break; 

case 5: 

iectf(70.0,0.0,110.0,10.0); 

RGBcolor(0,0,255); 

cinov2(88.0.1.5); 







charstrCS"); 

break; 


ddEuilt; 

rectf(70.0.40.0,110.0, 50.0); 

RGBcolor(0,0,255); 

cmov2(88.0,41.S); 

charstr("l"); 

break; 

} 

// Velocity 
RGBcolor(0,0,255); 
rectf(200.0,20.0,230.0, 50.0); 
RGBcolor(200,200,200); 
rectfl[190.0,20.0,200.0, 50.0); 
rectf(230.0,20.0,240.0, 50.0); 

// Draw up and down arrows 
RGBcolor(0,0,0); 
poIf2(3,pt); 
polf2(3,pt2); 

pt[01[ll = pt[0]Il]-10.0; 
ptllim = ptlllll]*10.0; 
pt{2Hll = ptI2][l]-10.0; 
pt2[0][ll = pt2[0][l].10.0; 
pt2[lHl]-pt2(ll[ll-10.0; 
pt2121flj=pt2(2jnj-10.0; 
poIC(3,pt); 
polC(3,pt2); 

ptl0][lI = pt[0Hl]-10.0; 

ptlll[l]”PtllHll-10.0; 

pt(2][lj-pt[2)ll)-10.0; 

pt2[0J[ll*pt2l0HlJ-10.0; 

pt21inil-pt2[ll[l]-10.0; 

pt2I2]Ill = pt2(21[l]-10.0; 

poli2(3,pt); 

pol£2(3,pt2); 

RGBcolor(255,255,0); 

cinov2(202.41); 

q)rintf(s, "%.ir, mag[0]); 

charstr(s); 

cnu>v2(202,31); 

q)rintf(s, "%.ir, mag[l]); 

cliarstr(s); 

cnK>v2(202,21); 

q)rintf(s, "%.ir, n]ag[2]); 

charstr(s); 


//Position 
RGBcolor(0,0,25S); 
iectfl[265.0,20.0,295.0, 50.0); 
RGBcolor(200,200,200); 
r«:tfl[255.0,20.0,265.0,50.0); 








rectf(295.0,20.0, 305.0, 50.0); 
// Draw up and down arrows 
RGBcolor(0,0,0); 
pt[0](01 = ptlOJIO] + 65.0; 
pt(ll[01 = pt(lU01 + 65.0; 
ptI21l01 = ptI21I01 + 65.0; 
pt2(01[0I«pt2I0][0] + 65.0; 
pG(lJI0J»pt2Il](0] + 65.0; 
pai21101 = pai21l01 + 65.0, 
poll2(3,pt); 
polf2(3,pt2); 

ptl0Hl] = pt[0Hll + 10.0; 
ptllUll = pniHl] + 10.0; 
pt[2Hll = pt[2Ull + 10.0; 
pt210JIlJ=pt2I0]Il] + 10.0; 
pt2IlHl] = pt2Ill[l) + 10.0; 
pt2f21[IJ-pt2[2Hl} + 10.0; 
pol£2(3.pt); 
polC(3,pt2); 

pt{01[l] = pt(01(ll+10.0, 
pt[im]=pt(ll[l] + 10.0; 
ptI2]Il]=pt[2HlJ+10.0; 
pt2(01[l] = pt2(0|Il] + 10.0; 
pt2llHll=pt2[ll[ll + 10.0; 
pt2[21(l] = pt2[2][l] + 10.0; 

polf2(3,pt); 
polf2(3,pt2); 
RGBcolor(255,255,0); 
cniov2(267,41); 
sprintf(s, "%.lf", pos(0]); 
charstr(s); 
cinov2(267, 31); 
q)rintf(s, "%.ir, pos[l]); 
charstr(s); 
anov2(267,21); 
sprintf(s, "%.ir, pos[2]); 
charstr(s); 

// Angular Velocity 
RGBcolor(0,0,255); 
rectf(330.0,20.0,360.0,50.0); 
RGBcolor(200,200,200); 
rectf(320.0,20.0,330.0, 50.0); 
rectf(360.0,20.0,370.0,50.0); 
//Draw up and down arrows 
RGBcolor(0,0,0); 
pt[0I[0J»pt[0I{0I + 65.0; 
pt[ll[0I-pt[ll[0J + 65.0; 
ptl21[01»pt[2J[0] + 65.0; 
pt2I0JI0J = pt2I0Jl01 + 65.0; 
pt2[lH0]-pt2(l][0]-t-65.0; 
pai2]l0] - pt2[2][0) + 65.0; 
pol£2(3,pt); 
polCZ(3,pt2)', 







ptl01(ll-i)t[0HlM0.0; 

pt[2111j»pt[21lll.l0.0; 

pt2IOHll-pt2I01[11.10.0; 

pt2111llj-pt2(ll[ll-10.0; 

pt2[21(l]-pt2l2H11.10.0; 

polf2(3.pt); 

polC(3.pt2); 

ptI0Hll*pt(01(11.10.0; 
pt[llIlJ = ptflJIlMO.O; 
ptmill'ptmill-lO.O; 
pt2I01[l] = pt2l0](ll-10.0; 
pt2(lHll=pt2llHll-10.0; 
pt2l2Hll»pt2l2Hll-10.0; 
pol£2(3.pt); 
pol£2(3,pt2); 

RGBcolor(255.255.0); 
cmov2(332,41); 
sprintf(s, av(Ol); 

charstrCs); 
cmov2(332. 31); 
sprintf(s, "%.ir, av(l]); 
charstr(s); 
cmov2(332,21); 
spruitf(s, "%.ir, av[2]); 
charstr(s); 

/Avindow text 

RGBcolor(0,0,0); 

cmov2(10.0,90.0); 

charstr("Particle Dynamics Control Window"); 

cinov2(20.0,75.0); 

charstr("View"); 

cmov2(20.0,65.0); 

charstr("Lever); 

cmov2(75.0,65.0); 

charstr("Frame"); 

cmov2(75.0.55.0); 

charstr("Lever); 

//cmov2(125.0,55.0); 

//dbarstr("Motion"); 

cniov2(185.0,65.0); 

charstr("Lincar"); 

chiov2(185.0,55.0); 

charstrCVelocity"); 

cmov2(255.0.55.0); 

diarstr^osition*); 

cinov2(320.0,65.0); 

charsirC Angular"); 

cniov2(320.0,55.0); 

charstr("Velocity"); 

ii(vlevel« 0) /Aiewing axes are only display if viewing level is not inertial 

{ 
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} 

else 

{ 

RGBcolor(0,0,0); 

cmov2(390.0,65.0); 

charstr(*View"); 

ciiiov2(390.0,55.0); 

charstr("Axis"); 

RGBcolor(0,0,255); 

rectf(390.0.20.0.430.0.50.0); 

RGBcolor(255.255.0); 

cinov2(394.0.41.0); 

charstr("-X"); 

cmov2(414.0.41.0); 

charstr("+X"); 

ciiiov2(394.0.31.0); 

charstrC-Y"); 

cmov2(414.0.31.0); 

chaistr(*+Y-); 

cmov2(394.0,21.0); 

charstr("-Z-); 

cmov2(414.0.21.0); 

charstr("+Z"); 


switch(vaxis) 

{ 

*3 * 

rcctf(390.0.20.0.410.0. 30.0); 

RGBcolor(0,0,255); 

cinov2(394.0.21.0); 

charstr("-Z"); 

break; 

case-2; 

rectfl[390.0.30.0,410.0,40.0); 

RGBcolor(0,0,255); 

cmov2(394.0.31.0); 

charstr("-Y"); 

break; 

case>l: 

rectf{390.0,40.0,410.0, 50.0); 

RGBcolor(0,0,255); 

ciiu;v2(394.0,41.0); 

charstr("-X"); 

break; 

case 1: 

rectf{410.0,40.0,430.0,50.0); 

RGBeolor(0,0,255); 

cinov2(414.0,41.0); 

charstr("+X"); 

break; 
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case 2. 

rectf(410.0. 30.0,430.0,40.0); 

RGBcolor(0,0,255); 

cmov2(414.0,31.0); 

chaistr("+Y"); 

break; 

case 3; 

rcctf(410.0,20.0,430.0, 30.0); 

RGBcolor(0,0,255); 

cmov2(414,0,21.0); 

charstr("+Z"); 

break; 

default; 

rectf(390.0,40.0,410.0, 50.0), 

RGBcolor(0,0,255); 

cmov2(394.0,41.0); 

charstrC-X"); 

break; 

} 

} 

popmatiixO; 

swapbuffersO; 

} 


void gyro_conttols(int x, int y, int z, iat object, vector3D size, int tl, int t2, int t3, double duration, 
double mag, double elapsed, double total) 

{ 

//x, y, z ate assigned angular velocities, object is the body type, 

//tl, t2, t3 are the applied moments 
float ptI3](21 = { 

{142,48}, 

{148,48}, 

{145,42}}; 
float pt2 [3][2] = { 

{182,42}, 

{188.42}. 

{185.48}}; 
winset(control_win); 
char s[32]; 
pushmatrixO; 

ortho2(0.0,769.0,0.0,100.0); 

RGBcolor(255,255,255); 

clearO; 

//Go & Reset Buttons 
RGBcolor(200,200,200); 
rectf(475.0,25.0,525.0,75.0); 
rectf(550.0,25.0,605.0,75.0); 
rectf(625.0.25.0,675.0,75.0); 
lectfC/OO.0,25.0,750.0,75.0); 
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RGBGOlor(0.0,0); 

cinov2(487. 50); 

charstr("Body''); 

cmov2(482.40); 

charstr("Moment"); 

ciiiov2(SSl. 50); 

cliarstr(”liiertial”); 

cinov2(558,40); 

chaistr(*Moiiient"); 

cmov2(639,50); 

charstr("Spin"); 

cinov2(643,40); 

charstr("up"); 

ciiiov2(710,45); 

charstr("Resct"); 

// Angular Velocity 

RGBcolor(0,0.255); 

lectHlSO.0.40.0,180.0,70.0); 

RGBcoIor(200,200,200); 

rectf(140.0, 40.0,150.0, 70.0); 

rcctf(180.0,40.0,190.0,70.0); 

// Draw up and down arrows 

RGBcolor(0,0,0); 

pol£2(3,pt); 

pol£2(3,pt2); 

pt(01[ll«ptl0]Il] + 10.0; 
pt[lim = pt(lHll + 10.0; 
ptl2J(ll-pt{2Ull+10.0; 
pt2I0Hll“pt2(0J(l] + 10.0; 
pt2[ll(ll*pt2[l][l] + 10.0; 
pai2Hll*pt2l21ll] + 10.0; 
poIC(3,pt); 
pol£2(3,pt2); 

ptl0]ll)=ptl0]llj + 10.0; 

pt[ll[ll = pt[lUll + 10.0; 

pt[21[ll = ptl2][l] + 10.0; 

pt2t0Hll»pt2(0]{l]+ 10.0; 

pt2[llll] = pt2[l]lll + 10.0; 

pt2[21Il] = pt2l2][l] + 10.0; 

polQ(3,pt); 

poIf2(3.pa); 

RGBcolor(255,255,0); 

cmov2(155,61); 

qnintRs, ”%.0f", (double) x); 

charstr(s); 

cinov2(155,51); 

spiintfCs, "%.0f", (double) y); 

charstiXs); 

cmov2(155,41); 

sprintf(s, "%.or, (double) z); 

c]iarstr(s); 


// External Moment 








RGBcolor(0.0.255); 
rectf(220.0,40.0,250.0,70.0); 
RGBcoIor(200,200.200); 
iectf(210.0, 40.0,220.0, 70.0); 
rectf(250.0,40.0,260.0,70.0); 
// Draw up and down arrows 
RGBcolor(0,0,0); 
pt[0)(01 = pt(01[01 + 70.0; 
pt[lJ(0I = prlllI0] + 70.0; 
pt[2110]=ptl2]{0] + 70.0; 
pt2[01[0]»pt2[01[01 + 70,0; 
pt2ll]I0]=pt2[llI01 + 70.0; 
pt2l2]l01 = pt2[21(0) + 70.0; 
polf2(3,pt); 
polf2(3,pt2); 

pt[0Hll = ptI0]Ill-10.0; 
pt[lHll»ptlllIll-10.0; 
ptI21[lJ = pt[2HlJ-10.0; 
pt2[01lll = pt2(0][ll-10.0; 
pt2[l](lj = pt2[l](ll-10.0; 
pt2[21lll = pt2(2]ll]-10.0; 
poIC(3,pt); 
polf2(3,pt2); 

ptI0]ll] = ptl0]Il].10.0; 

ptmm=ptlll[l]-10.0; 

pt[2Hll=ptI2Ull-10.0; 

pt2[0]lll = pt2t0][l]-10.0; 

pt2IlHll = pt2[l][ll-10.0; 

pt2[21tlJ = pt2l2Hll-10.0; 

polf2(3,pt); 

pol£2(3,pt2); 

RGBcolor(255,255,0); 

cmov2(225,61); 

q)rintf(s, "%.or, (double) tl); 

charstr(s); 

cmov2(225, 51); 

q)rintf(s, "%.or, (double) t2); 

charstr(s); 

cmov2(225,41); 

sprintf(s, "%.0f", (double) t3); 

charstr(s); 

// Duration & Magnitude 
RGBcolor(0,0,255); 
rectf(290.0,60.0,330.0,70.0); 
rcctK290.0,40.0,330.0,50.0); 
RGBcolor(200,200,200); 
rectf(280.0,60.0,290.0,70.0); 
rectfl[330.0.60.0,340.0,70.0); 
tectf(280.0,40.0,290.0,50.0); 
rectf(330.0,40.0,340.0,50.0); 
//Draw up and down arrows 
RGBcolor(0,0,0); 
pt[01[01-pt[01I0] + 70.0; 






pt(IH01-pt[lJ[0I + 70.0; 
ptl21[01-ptl21[0I + 70.0; 
pa(01l01-ptt(0]l01 + 80.0; 
pt2[lJ(0J»pt2{l}f0] + 80.0; 
pt2l2U01 = pGf21101 + 80.0; 
pol£Z(3,pt); 
pol&(3.i^); 

ptlOUll»pt[01[ll + 20.0; 
ptmUl-pt{lI[ll + 20.0; 
ptI2HlJ-ptl21IlJ + 20.0; 
pt2[01(l]-pQl0Hll + 20.0; 
pt2[llllJ-pt2[lJ[ll + 20.0; 
pt2(2Hl]«pt2I21[ll + 20.0; 
polf2(3.pt); 
poI£2(3.pt2); 

RGBcolor(255,255,0); 
cmov2(29f^, 61); 
sprint^s, duration); 

charstr(s); 
ciiiov2(295,41); 

^rintf(s, "%.0c", mag); 
charsti(s); 

//Clocks 

RGBcolor(255,0,0); 
itctf(355.0.10.0,460.0. 80.0); 

RGBcoMO.O.O); 
cmov2(365.0,71.0): 
cliar5tF(”Sessioa Time"); 
cmov2(360.0,41.0); 
chaistr("Moment Applied"); 
RGBcolor(255.255,235); 

cmov2(380,60); 

^rintf(s, "%.ir, total); 
chaisti(s); 
cmofv2(380, 30); 
q>iintf{s. "%.2r, elapsed); 
diarstr(s); 

//window text 

RGBcolor(0.0,0); 

cmov2(10.0,90.0); 

cbaistr("GyToscq>ic Stiffiiess Control Window"); 

cinov2(10.0,75.0); 

diafStr("Sliape”); 

cmov2(92.0,75.0); 

diarstrCSize*); 

anov2(140.0,75.0); 

charstr("Ang Vd"); 

cniov2(197.0,61.5); 

charstrTX"); 

cmov2(197.0.51.5); 

chantrCY"); 





cinov2(197.0.41.5); 

chatstr("Z"); 

cmov2(215.0.75.0); 

charstr("Moment"); 

cniov2(280.0,71.0); 

charstr(”Duration”); 

cmov2(280.0.51.0); 

charstr("Magnitude'*); 

// Select between the 3 base objects 

RGBcolor(0.0.255); 

rectf(10.0.40.0.70.0,70.0); 

RGBcolor(255.255.0); 

cmov2(lS.0,61.S); 

charstrCSphere"); 

cmov2(15.0,51.5); 

charstr("Block"); 

cinov2(l 5.0,41.5); 

charstr{"Cylinder"); 

switch(obJect) 

{ 

case 1: 

rectfl[10.0.60.0,70.0,70.0); 
RGBoolor(0,0,255); 
cinov2(15.0,61.5); 
charstr("Sphere"); 
break; 

case 2: 

rectHlO.O, 50.0,70.0,60.0); 

RGBcolor(0.0,255); 

cmov2(15.0,51.5); 

charstr("61ock"); 

break; 

case 3: 

rectf(10.0,40.0,70.0,50.0); 

RGBcolor(0,0.255); 

cinov2(15.0,41.5); 

charstr("Cylinder"); 

break; 

d^ult: 

rectf(10.0,60.0,70.0,70.0); 

RGBcolor(0.0,255); 

cmov2(15.0,61.5); 

diaistr("Sphere"); 

break; 


> 

//Object Size 





RC5Bcolor(0.0.255); 
iectf(90.0,40.0,120.0,70.0); 

RGBcolor(200,200.200); 
rectf(80.0.40.0,90.0,70.0); 
rectf(120.0, 40.0, 130.0,70.0); 

// Draw up and down anows 
RGBcolor(0,0,0); 
pt[0Jl0] = ptlOllO) - 200.0; 
l«llll01 = pt[ll(01-200.0; 
pt[21[01»ptI2]I0]-200.0; 
pt2l0H01=pt2J0]l0]-210.0; 
pt2(ll(01 = pt2ni(01-210.0; 
pt2l21[01 = pt2t2][01-210.0; 
polf2(3,pt); 
poli2(3.pt2); 

ptI0)IlJ = ptI0]Il]-10.0; 
ptUnil-ptllHll-lO.O; 
pt(21[ll = pt[21[ll-10.0; 
pt2(0][l] = pt2(0J(ll-10.0; 
pGllim = pt211]lll-10.0; 
pt2(2Ull = pt2I21[ll-10.0; 
pol£2(3.pt); 
pott2(3.pt2); 

pt[01(ll“pt[0Hll-10.0; 

pt(l][ll=ptIlHl]-10.0; 

pt{2Hll-ptI2Hl}-10.0; 

pt2I0111]“pt2l01[ll-10.0; 

Pt2IllllJ = pt2Il][ll-10.0; 
pa(21(lJ = pt2l2Hll-10.0; 
poie(3,pt); 
polC(3.pt2); 

RGBcolor(255,255,0); 
cmov2(91,61); 
q)iintf(s, "%.ir, size[0]); 
chaistr(s); 
cmov2(91,51); 
q>rintf)[s, •%.ir, size[l]); 
charstT(s); 
cmov2(91,41); 
q)rint^s, size[2)); 

diarstT(s); 
pc^matrixO; 
swapbuffersO; 

> 

void stat_controIs(doiible x. double y, double z, double am, double mass, double Ix, double ly, double Iz, 
vectorSD* oId_av) 

{ 

inti; 

winset(main_win); 
cbar s[32]; 

RGBcolor(25S.0,0); 
onovsCIO, 15, -30); 
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charstr("X velocity"); 
cniovs(15,15, -30); 
q»rintf(s, "%.3r, x); 
charstr(s); 
anovs(-20,14, -30); 
charstr("lxx"); 
cmovs(-18,14, -30); 
sprintf(s, ■%.ir, Ix); 
charstr(s); 

RGBcoIor(0,0,2S5); 
cmovs(10,14, -30); 
charstr(*Y velocity"); 
cinovs(15, 14, -30); 
sprintf(s, "%.3r, y); 
charstr(s); 
cmovs(-20, 13, -30); 
charstr("Iyy"); 
cmovs(-18,13, -30); 
sprintf(s, "%.ir, ly); 
chaTStr(s); 

RGBcolor(0.0.0); 
cmovs(10,13, -30); 
charstr(*Z velocity"); 
cmovs(lS, 13, -30); 
sprintf(s, "%.3r, z); 
charstr(s); 
ciiiovs(-20, 12, -30); 
cliarstr("Izz"); 
cmovs(-18,12, -30); 
q)rintf(s, "%.ir, Iz); 
charstr(s); 

cniovs(10,12, -30); 
chaTStr("Angular Momentum"); 
cmovs(17,12, -30); 
sprintf(s, "%.ir, am); 
charstr(s); 


cmovs(-20,15, -30); 
charstr("Mass"); 
cmovs(-18,15, -30); 
q>rintf(s, "%.ir, mass); 
chaistr(s); 

RGBcoloT(200,200,200); 
iectf(-3.2, -4.1, 3.25, -1.8); 
RGBcolor(100,100,100), 
iectf(-2.8, -3.01,3.25, -2.99); 


RGBcolor(0,0,0); 
cmovs(-3, -3,0); 
diaistF(”0”); 








cinovs(-3, -2,0); 
charstr(*10*); 
cinovs(-3, -4,0); 
charstr("-10*); 

for(i=0;i<300;i++) //draw x graph 
RGBcolor(255.0,0); 

if((old_av(i])I0J < 10.0 && (old_av(ij)[01 > -10.0) 

iectf(-2.75 + 0.02 • i,-2.99 + .1 * (old_av[il)10],-2.73 + 0.02 • i,-3.01 + .1 * (old_avlil)101); 
else 

if((old_av[il)(0J<0,0) 

iectf(-2.75 + 0.02 • i,-3.99,-2.73 + 0.02 * i.-4.01); 
else 

rectf(-2.75 + 0.02 • i,-1.99,-2.73 + 0.02 ♦ i.-2.01); 

RGBcolor(0,0,2SS); //draw y graph 
if((old_avli])Ill < 10.0 && (old_av[i])Il] > -10.0) 

iectfl[-2.75 + 0.02 * i,-2.99 + .1 * (old_av{il){ll.-2.73 + 0.02 * i,-3.01 + .1 * (old_av(il){ll); 
else 

if((old_av[i])ll] < 0.0) 

re^-2.75 + 0.02 • i.-3.99.-2.73 + 0.02 * i.-4.01); 
else 

rectf(-2.75 + 0.02 * i,-1.99,-2.73 + 0.02 ♦ i,-2.01); 

RGBcolor(0,0,0); //draw z graph 
if((old^avlil)(21 < 10.0 &&. (old_av[il)[2] > -10.0) 

iectf(-2.75 + 0.02 * i,-2.99 + .1 • (old_av(i])l2],-2.73 + 0.02 * i,-3.01 + .1 * (old_av[i])[2J); 
else 

if((old_av[il)(21<0.0) 

iectf(-2.75 + 0.02 * i,-3.99,-2.73 + 0.02 ♦ i,-4.01); 
else 

rcctf(-2.75 + 0.02 * i,-1.99,-2.73 + 0.02 * i,-2.01); 

} 

} 

void attach eye to(vector3D* v, int* i) 

{ 

if {eye display field != NULL) 

{ 

*eyc display field » I; 

} 

eye*v, 

eyejdispl^^field = i; 

*eye_displ^_fidd = 0; 
twist “0; 


void attacfa_taiget to(vector3D* v) 

{ 

tai^ = v; 
twist“0; 

> 
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void set eye to(double x, double y, double z) 

{ 

if (eye diq>lay field !== NULL) 

{ 

*eye_display_field = 1; 
eye dispiay_ficld = NULL; 

} 

* new vector3D(x, y. z); 
twist “0; 


void set target_to(double x, double y. double z) 

{ 

target = new vcctor3D(x, y, z); 
twist =»0; 


void viewQ 

{ 

swapbufiersQ; 

czclear(0xFFFF7200,getgdesc(GD_ZMAX)); 

loadinatrix(un); 

perspective(450,1.25,0.2,10000.0); 

lodcat((*eye)(0],(*eye)[ll,(*cye)[21,(^target)(01,(*target)[lj,(*target)[2], (int) (twist • 
572.957795131)); 

display_this_object(lightobj); 


void view(quatemion q, vector3D new_eye, int view_axis) 

{ 

Matrix rt = { 1.0,0.0,0.0,0.0, 

0 . 0 , 1 . 0 , 0 . 0 , 0 . 0 , 

0 . 0 , 0 . 0 , 1 . 0 , 0 . 0 , 

0 . 0 , 00 , 0 . 0 , 1 . 0 }; 
inatrix3x3 rotation, axis; 
swapbufiersQ; 

czclear(0xFFFF7200,getgdesc(GD_ZMAX)); 

loadinatrix(un); 

perspective(450,1.25,0.2,10000.0); 
swit^(view axis) 

{ 

//Negative Y axis 
case-2: 

axis.DCM_x_rotalion(1.5707963268); 

break; 

//Negative X axis 
case-1: 

axis.DCM_y_iotation(-1.5707963268); 

break; 
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//Positive X axis 
case 1: 

axis.DCM_y_rotation( 1.5707963268); 
break; 

//Positive Y axis 
case 2: 

axis.DCM_x_rotation(-1.5707963268); 
break; 

//Positive Z axis 
case 3: 

axis.DCM_y_rotation(3.14159265359); 
break; 

default; 

break; 

} 

rotation.DCM_body_to_world(q); 
rotation = rotation * axis; 
new_eye = new_eye * -1; 

// load rotation elements 
rt(0](0] = rotation(0]; 

^ni[0] = rotation(3]; 
ttbilo] = rotationidj; 
rt[31(01-0.0; 

Tt[0j(l]» rotationfl]; 
rthlill ” rotation^]; 
rt(2](l] = rotation(7]; 
rt{3J[ll = 0.0; 

rotation[2]; 
rt[lj[2]»Totation[5j; 
rt[2][2] = tx>tation[8]; 
rt[3J[2] = 0.0; 
rtI3](0I = 0; 
rtl3]Ill = 0; 
rt[3]l2] = 0; 
rt[3H3) = 1.0; 

multinatrix(rt); //rotateview 
// load translational matrix 
rt[01t01 = 1; 
rt[ll(0] = 0; 
rt(21[0J = 0; 
rt[3JI0] = 0; 
rtI01[l] = 0; 
rt[llll] = l; 
rtI21[ll»0; 
rtl3I[ll = 0; 
rt[0]t21 = 0; 
rtllj[21 = 0; 
rtl21I2]*l; 
rtI3J[2] = 0; 
rtPltO] = new_eye[0J; 
rtpill] “ new_eye[li; 






rtl3Jl21 = new eye[2]; 
rt[31[31 = 1.0;~ 

multmatrix(rt); //translate eye point 
display this_object(light(AJ); 

} 

void view axisQ 

{ 

display_this_object(axis); 


void rotate_view(int angle) 

{ 

twist = angle; 

} 

#endif 



APPENDIX J 


Menu Code 


A. HEADER FILE 
#ifndefMENU H 
#definc MENU_H 
#uiclude "menu.H" 
#uiclude <gl.h> 
#mclude <dcvice.h> 
^include <stdio.h> 

1 initialize_mcnuO; 

^ ^ueuejestO; 

#endif 
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B. SOURCE FILE 
#ifiidefMENU_C 
#define MENU_C 
#include "menu.H” 

#include "time.H* 

#inclu<le <iostieain.h> 

long mainmenu; 
long hititem; 
short value; 
double wx, wy; 

long makethemenusO 

I* this routine performs all the menu construction calls */ 

{ 

long topmenu; 

topmenu * deipup(”Dynamics Visuali7er %t | Exit %x99"); 
retum(topmenu); 

} 

void initialize menuO 

{ 

mainmenu = makethemenusQ; //GL function 

} 

void pTocessmenuhit(long hititem) 

{ 

switchChititem) 

{ 

case 99: 
exit(0); 
break; 

default: 

break; 

} /* end switch *! 

} 

int queue testQ 

{ 

hititem = 0; 

while(qtestO) 

{ 

switch(qread(&value)) 

{ 

case MENUBUTTON: 
if(value = 1) 

{ 

mmodefMSINGLE); 
hititem dopup(mainmenu); 
mmode(MVIEWING); 
processmenuhit(hititem); 
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reset timeO; 

} 

break; 

case LEFTMOUSE: 

wx = getvaluator(MOUSEX); 

wy “ g^valuatorCMOUSEY); 

hititem »(wx * 100000) + wy ; //encode mouse location 

break; 

case REDRAW: 
reshapeviewportO; 
break; 

case UPARROWKEY: 
hititem = 100; 
break; 

case DOWNARROWKEY: 
hititem * 101; 
break; 

case LEFTARROWKEY: 
hititem 102; 
break; 

case RIGHTARROWKEY: 
hititem = 103; 
break; 

cascEQUALKEY: 
hititem = 104; 
break; 

case MINUSKEY: 
hititem = 105; 
break; 

case SPACEKEY: 
hititem »106; 
break; 

cascFlKEY: 
hititem «111; 
break; 

caseF2KEY; 
hititem 112; 
break; 

caseF3KEY: 
hititem «113; 
break; 
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caseF4KEY: 
hititem= 114; 
break; 

caseFSKEY: 
hititem = 115; 
break; 

caseF6KEY; 
hititem = 116; 
break; 

caseFTKEY: 
hititem® 117; 
break; 

caseFSKEY: 
hititem® 118; 
break; 

caseF9KEY: 
hititem® 119; 
break; 

caseFlOKEY; 
hititem = 120; 
break; 

caseFllKEY: 
hititem® 121; 
break; 

cascF12KEY: 
hititem = 122; 
break; 

de&ult: 

break; 

} 

} 

return (int) hititem; 

} 

#endif 
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